Statistics
| Revision:

root / branches / simulator / projects / simulator / simulator / core / motion.c @ 1006

 1 ```#include ``` ```#include ``` ```#include "motion.h" ``` ```#define CUTOFF 120 ``` ```#define ABS(x) (x<0?-x:x) ``` ```#define TIME 1 /*sec*/ ``` ```#define ROBOT_WIDTH 131 /*mm*/ ``` ```/** move 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 function instead of returning a value ``` ``` * (x,y) is some kind of absolute position in the "world", let's make (0,0) the top left of the "world" ``` ``` * theta will an angle be between 0 - 2*Pi (0 being faces east) ``` ``` * speed is between 0 - 255, there is some magical cutoff point before the motors actually starts running ``` ``` * move will return 0 if successful ``` ``` **/ ``` ```int move (int *x, int *y, double *theta, int speed1, int speed2) { ``` ``` if (*theta < 0 || *theta > 2*M_PI) return 1; ``` ``` if (speed1 < 0 || speed1 > 255) return 1; ``` ``` if (speed2 < 0 || 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 */ ``` ``` *x += cos(*theta) * speed1; ``` ``` *y += sin(*theta) * speed1; ``` ``` return 0; ``` ``` } ``` ``` radius = ROBOT_WIDTH * speed1 / (speed1 - speed2); ``` ``` ``` ``` double t = speed1 / radius; ``` ``` double newx = radius * sin(t); ``` ``` double newy = radius - radius * cos(t); ``` ``` *x += newx * cos(*theta); ``` ``` *y += newx * sin(*theta); ``` ``` *x += newy * - sin(*theta); ``` ``` *y += newy * cos(*theta); ``` ``` *theta = fmod((t + *theta), (2 * M_PI)); ``` ``` if (*theta<0) *theta += 2 * M_PI; ``` ``` return 0; ``` ```} ```