Statistics
| Revision:

## root / branches / simulator / projects / simulator / core / test_motors.c @ 985

 1 ```#include ``` ```#define FORWARD 1 ``` ```#define BACKWARD 0 ``` ```#define CUTOFF 120 ``` ```/* move will move a robot from its initial position, (x,y), and theta (in degree) to a new position given dir, and speed. ``` ``` * (x,y) 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 - 359 (0 degree being faces east) ``` ``` * dir is either FORWARD or BACKWARD ``` ``` * speed is between 0 - 255, there is some magical cutoff point before the motors actually starts running ``` ``` * 2 pairs of dir and speed, one for each motor ``` ``` * move will return 0 if successful ``` ``` */ ``` ```int move (int *x, int *y, int theta, int dir1, int speed1, int dir2, int speed2) { ``` ``` if (theta < 0 || theta > 359) return 1; ``` ``` if (dir1 != FORWARD && dir1 != BACKWARD) return 1; ``` ``` if (dir2 != FORWARD && dir2 != BACKWARD) 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 (speed1 < CUTOFF) { ``` ``` speed1 = 0; ``` ``` } ``` ``` if (speed2 < CUTOFF) { ``` ``` speed2 = 0; ``` ``` } ``` ``` /* FAKE VALUES */ ``` ``` (*x) += speed1-CUTOFF; ``` ``` (*y) += speed1-CUTOFF; ``` ``` /* d = distance between the two wheels = 12 cm ``` ``` * v1 = speed of the first wheel ``` ``` * v2 = speed of the second wheel ``` ``` * ``` ``` * Unless v1 = v2, if left at that point, the robot will go in a circle at some radius. ``` ``` * ``` ``` * This radius is = dv1/(v1-v2). ``` ``` * Since we could measure the displacement of a wheel, we could figure out what the overall angular displacement is using r(theta) = arc length and then use trig after that to figure out x, y coordinate displacement. ``` ``` */ ``` ``` return 0; ``` ```} ``` ```int main () { ``` ``` int x = 0; ``` ``` int y = 0; ``` ``` printf("%d %d\n",x,y); ``` ``` move(&x,&y,0,FORWARD,150,FORWARD,150); ``` ``` printf("%d %d\n",x,y); ``` ``` return 0; ``` ```} ```