Statistics
| Revision:

## root / branches / simulator / projects / simulator / simulator / core / motion.c @ 1091

 1 ```#include ``` ```#include ``` ```#include ``` ```#include "motion.h" ``` ```#include "rangefinders.h" ``` ```#include "robot.h" ``` ```#define CUTOFF 120 ``` ```#define TIME 1 /*sec*/ ``` ```#define ROBOT_WIDTH 131 /*mm*/ ``` ```#define MOTOR_CONVERSION_FACTOR 10.0 ``` ```#define FUDGE 10 /* minimum rangefinder distance until collision */ ``` ```/** move_robot will move a robot from its initial position, (x,y), and theta (in radians) to a new position given speed. ``` ``` * (x,y) and theta will be updated by the move_robot function instead of returning a value ``` ``` * (x,y) is some kind of absolute position in the "world", (0,0) is the top left of the "world" ``` ``` * theta will an angle be between 0 and 2*Pi (0 being faces east and goes clockwise) ``` ``` * speed is between 0 and 255, there is some magical cutoff point before the motors actually starts running ``` ``` * move will return 0 if successful ``` ``` **/ ``` ```int move_robot(Robot* r) ``` ```{ ``` ``` Pose old_pose = r->pose; ``` ``` ``` ``` short speed1 = r->shared->motor1; ``` ``` short speed2 = r->shared->motor2; ``` ``` float theta = r->pose.theta; ``` ``` ``` ``` if (theta < 0 || theta > 2*M_PI) return 1; ``` ``` if (speed1 < -255 || speed1 > 255) return 1; ``` ``` if (speed2 < -255 || speed2 > 255) return 1; ``` ``` /* if speed is lower than the cut off, don't move */ ``` ``` if (abs(speed1) < CUTOFF) { ``` ``` speed1 = 0; ``` ``` } ``` ``` if (abs(speed2) < CUTOFF) { ``` ``` speed2 = 0; ``` ``` } ``` ``` double radius; ``` ``` if (speed1 == speed2) { ``` ``` /* go straight */ ``` ``` r->pose.x += cos(theta) * speed1 / MOTOR_CONVERSION_FACTOR; ``` ``` r->pose.y += sin(theta) * speed1 / MOTOR_CONVERSION_FACTOR; ``` ``` return 0; ``` ``` } ``` ``` radius = ROBOT_WIDTH * speed1 / (speed1 - speed2); ``` ``` ``` ``` double t = speed1 / radius / MOTOR_CONVERSION_FACTOR; ``` ``` double newx = (radius * sin(t)) / MOTOR_CONVERSION_FACTOR; ``` ``` double newy = (radius - radius * cos(t)) / MOTOR_CONVERSION_FACTOR; ``` ``` ``` ``` r->pose.x += newx * cos(theta); ``` ``` r->pose.y += newx * sin(theta); ``` ``` r->pose.x += newy * - sin(theta); ``` ``` r->pose.y += newy * cos(theta); ``` ``` ``` ``` int divide = (t+r->pose.theta)/(2 * M_PI); ``` ``` r->pose.theta = (t+r->pose.theta) - (2 * M_PI * divide); ``` ``` if (r->pose.theta<0) r->pose.theta += 2 * M_PI; ``` ``` /* XXX: this is a terrible hack */ ``` ``` update_rangefinders(r); ``` ``` for (divide = 0; divide < 5; divide++) { ``` ``` /* Lets just call this a collision... */ ``` ``` if (r->shared->ranges.d[divide] < FUDGE) { ``` ``` /* Restore x,y, but allow rotation */ ``` ``` r->pose.x = old_pose.x; ``` ``` r->pose.y = old_pose.y; ``` ``` /* Rotated robot, need to recalculate */ ``` ``` update_rangefinders(r); ``` ``` return 0; ``` ``` } ``` ``` } ``` ``` return 0; ``` ```} ```