Project

General

Profile

Revision 900

Added by Chris Mar over 15 years ago

added remote control to robot code that sends mapping data

View differences:

trunk/code/projects/mapping/robot/robot_main.c
8 8
#define POINT_RAW 1 // packet type for map data points w/ raw encoder data
9 9
#define POINT_ODO 2 // packet type for map data points w/ odometry data
10 10

  
11
#define WL_CNTL_GROUP 4
12

  
13
#define FORWARD 0
14
#define BACK    1
15
#define LEFT    2
16
#define RIGHT   3
17
#define LEFT_CURVE 4
18
#define RIGHT_CURVE 5
19
#define STOP    6
20
#define VEL_UP  7
21
#define VEL_DOWN 8
22

  
23
int state;
24
int velocity;
25
char buf[100];
26

  
27
void packet_receive (char type, int source, unsigned char* packet, int length);
28
void set_motion (void);
29

  
30
PacketGroupHandler packetHandler = {WL_CNTL_GROUP, NULL, NULL, &packet_receive, NULL};
11 31
PacketGroupHandler love_handle = {MAP, NULL, NULL, NULL, NULL};
12 32

  
13 33
int main(void)
......
18 38
    wl_init();
19 39
    wl_set_channel(0xF);
20 40
    wl_register_packet_group(&love_handle);
41
    wl_register_packet_group(&packetHandler);
42
    orb_enable();
21 43
    //data_response_init();
22 44
    //wl_token_ring_register();
23 45
    //wl_token_ring_join();
24
    run_around_init();
46
    //run_around_init();
25 47
    //odometry_init();
26 48

  
49
    velocity = 160;
50
    state = STOP;
51

  
27 52
    //int x, y; // use when sending odometry data
28 53
    //float theta;
29 54

  
......
32 57

  
33 58
    while(1)
34 59
    {
35
	//wl_do();
36
	run_around_FSM();	/* traverse the environment and avoid obstacles ... */
60
	wl_do();
61
	//run_around_FSM();	/* traverse the environment and avoid obstacles ... */
37 62
	delay_ms(POLLING_INTERVAL);	/* until it's time to grab map data */
38 63
	//x = odometry_dx(); // use when sending odometry data
39 64
	//y = odometry_dy();
......
80 105
    return 0;
81 106
}
82 107

  
108
void packet_receive (char type, int source, unsigned char* packet, int length) {
109
    sprintf(buf, "received packet: %d\n", type);
110
    usb_puts(buf);
111
    switch (type) {
112
        case FORWARD:
113
            state = FORWARD;
114
            break;
115
        case BACK:
116
            state = BACK;
117
            break;
118
        case LEFT:
119
            state = LEFT;
120
            break;
121
        case RIGHT:
122
            state = RIGHT;
123
            break;
124
        case LEFT_CURVE:
125
            state = LEFT_CURVE;
126
            break;
127
        case RIGHT_CURVE:
128
            state = RIGHT_CURVE;
129
            break;
130
        case STOP:
131
            state = STOP;
132
            break;
133
        case VEL_UP:
134
            if (velocity > 250)
135
                velocity = 250;
136
            else
137
                velocity += 5;
138
            break;
139
        case VEL_DOWN:
140
            velocity -= 5;
141
            if (velocity < 150)
142
                velocity = 150;
143
            break;
144
        default:
145
            state = STOP;
146
            break;
147
    }
148
    set_motion();
149
}
150

  
151
void set_motion (void) {
152
    switch (state) {
153
        case FORWARD:
154
            orb_set_color(GREEN);
155
            move(velocity, 0);
156
            break;
157
        case BACK:
158
            orb_set_color(BLUE);
159
            move(-velocity, 0);
160
            break;
161
        case LEFT:
162
            orb_set_color(ORANGE);
163
            motor1_set(0, velocity);
164
            motor2_set(1, velocity);
165
            break;
166
        case RIGHT:
167
            orb_set_color(YELLOW);
168
            motor1_set(1, velocity);
169
            motor2_set(0, velocity);
170
            break;
171
        case LEFT_CURVE:
172
            orb_set_color(MAGENTA);
173
            motor1_set(1, 150);
174
            motor2_set(1, velocity);
175
            break;
176
        case RIGHT_CURVE:
177
            orb_set_color(CYAN);
178
            motor1_set(1, velocity);
179
            motor2_set(1, 150);
180
            break;
181
        case STOP:
182
            orb_set_color(RED);
183
            move(0, 0);
184
            break;
185
    }
186
}
187

  

Also available in: Unified diff