Statistics
| Revision:

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

History | View | Annotate | Download (2.83 KB)

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

    
5
#include "motion.h"
6
#include "rangefinders.h"
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 1000.0
14

    
15
#define FUDGE 10 /* minimum rangefinder distance until collision */
16

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

    
37
        /* if speed is lower than the cut off, don't move */
38
        if (abs(speed1) < CUTOFF) {
39
                speed1 = 0;
40
        }
41
        if (abs(speed2) < CUTOFF) {
42
                speed2 = 0;
43
        }
44

    
45
        double radius;
46
        int divide;
47
        if (speed1 == speed2) {
48
          /* go straight */
49
          r->pose.x += cos(theta) * speed1 / MOTOR_CONVERSION_FACTOR;
50
          r->pose.y += sin(theta) * speed1 / MOTOR_CONVERSION_FACTOR;
51
          for (divide = 0; divide < 5; divide++) {
52
            /* Lets just call this a collision... */
53
            printf("%d: %d\n",divide,r->shared->ranges.d[divide]);
54
            if (r->shared->ranges.d[divide] < FUDGE) {
55
                /* Restore x,y, but allow rotation */
56
                r->pose.x = old_pose.x;
57
                r->pose.y = old_pose.y;
58

    
59
                /* Rotated robot, need to recalculate */
60
                update_rangefinders(r);
61
                return 0;
62
            }
63
        }
64
          return 0;
65
        }
66
        radius = ROBOT_WIDTH * speed1 / (speed1 - speed2);
67
        
68
        double t = speed1 / radius / MOTOR_CONVERSION_FACTOR;
69

    
70
        double newx = (radius * sin(t)) / MOTOR_CONVERSION_FACTOR;
71
        double newy = (radius - radius * cos(t)) / MOTOR_CONVERSION_FACTOR;
72
        
73
        r->pose.x += newx * cos(theta);
74
        r->pose.y += newx * sin(theta);
75

    
76
        r->pose.x += newy * - sin(theta);
77
        r->pose.y += newy * cos(theta);
78
        
79
        divide = (t+r->pose.theta)/(2 * M_PI);
80
        r->pose.theta = (t+r->pose.theta) - (2 * M_PI * divide);
81
        if (r->pose.theta<0) r->pose.theta += 2 * M_PI;
82

    
83
        /* XXX: this is a terrible hack  */
84
        update_rangefinders(r);
85
        for (divide = 0; divide < 5; divide++) {
86
            /* Lets just call this a collision... */
87
            printf("%d: %d\n",divide,r->shared->ranges.d[divide]);
88
            if (r->shared->ranges.d[divide] < FUDGE) {
89
                /* Restore x,y, but allow rotation */
90
                r->pose.x = old_pose.x;
91
                r->pose.y = old_pose.y;
92

    
93
                /* Rotated robot, need to recalculate */
94
                update_rangefinders(r);
95
                return 0;
96
            }
97
        }
98

    
99
        return 0;
100
}
101