Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (1.86 KB)

1
#include <stdio.h>
2
#include <stdlib.h>
3
#include <math.h>
4

    
5
#include "motion.h"
6

    
7
#include "robot.h"
8

    
9
#define CUTOFF 120
10
#define TIME 1 /*sec*/
11
#define ROBOT_WIDTH 131 /*mm*/
12

    
13
#define MOTOR_CONVERSION_FACTOR 10.0
14

    
15
/** move_robot will move a robot from its initial position, (x,y), and theta (in radians) to a new position given speed.
16
 * (x,y) and theta will be updated by the move_robot function instead of returning a value
17
 * (x,y) is some kind of absolute position in the "world", (0,0) is the top left of the "world"
18
 * theta will an angle be between 0 and 2*Pi (0 being faces east and goes clockwise)
19
 * speed is between 0 and 255, there is some magical cutoff point before the motors actually starts running
20
 * move will return 0 if successful
21
 **/
22
int move_robot(Robot* r)
23
{
24
        short speed1 = r->shared->motor1;
25
        short speed2 = r->shared->motor2;
26
        float theta = r->pose.theta;
27

    
28
        if (theta < 0 || theta > 2*M_PI) return 1;
29
        if (speed1 < 0 || speed1 > 255) return 1;
30
        if (speed2 < 0 || speed2 > 255) return 1;
31

    
32
        /* if speed is lower than the cut off, don't move */
33
        if (abs(speed1) < CUTOFF) {
34
                speed1 = 0;
35
        }
36
        if (abs(speed2) < CUTOFF) {
37
                speed2 = 0;
38
        }
39

    
40
        double radius;
41
        if (speed1 == speed2) {
42
          /* go straight */
43
          r->pose.x += cos(theta) * speed1 / MOTOR_CONVERSION_FACTOR;
44
          r->pose.y += sin(theta) * speed1 / MOTOR_CONVERSION_FACTOR;
45
          return 0;
46
        }
47
        radius = ROBOT_WIDTH * speed1 / (speed1 - speed2);
48
        
49
        double t = speed1 / radius / MOTOR_CONVERSION_FACTOR;
50

    
51
        double newx = (radius * sin(t)) / MOTOR_CONVERSION_FACTOR;
52
        double newy = (radius - radius * cos(t)) / MOTOR_CONVERSION_FACTOR;
53
        
54
        r->pose.x += newx * cos(theta);
55
        r->pose.y += newx * sin(theta);
56

    
57
        r->pose.x += newy * - sin(theta);
58
        r->pose.y += newy * cos(theta);
59
        
60
        int divide = (t+r->pose.theta)/(2 * M_PI);
61
        r->pose.theta = (t+r->pose.theta) - (2 * M_PI * divide);
62
        if (r->pose.theta<0) r->pose.theta += 2 * M_PI;
63

    
64
        return 0;
65
}
66