Project

General

Profile

Revision 979

Added by Chris Mar about 15 years ago

minor changes to remote control / mapping code...wireless control works with libwireless reverted to rev. 887. code in odometry interrupt conflicts with motors, not allowing the robot to move motors in response to commands.

View differences:

trunk/code/projects/mapping/odometry/odometry.c
5 5
#include <avr/interrupt.h>
6 6
#include <dragonfly_lib.h>
7 7

  
8
void modify_velocity(void);
9

  
10

  
8 11
long lround(double d);
9 12

  
10 13
char control_velocity = 0;
......
148 151
}
149 152

  
150 153
void modify_velocity(){
151
	int diff = target_velocity - velocity;
154
	usb_puts("modify_velocity\n");
155
    int diff = target_velocity - velocity;
152 156
	if(diff > 0){
153 157
		move_speed++;
154 158
		move(move_speed,move_angle);
......
160 164
}
161 165

  
162 166
ISR(TIMER2_COMP_vect){
163
	odometry_run(); 
167
	//odometry_run(); 
164 168
}
165 169

  
166 170
long lround(double d){
trunk/code/projects/mapping/robot/robot_main.c
3 3
#include <encoders.h>
4 4
#include "odometry.h"
5 5

  
6
#define CHAN 0xE
7

  
6 8
#define POLLING_INTERVAL 50    //interval in ms to get sensor data
7 9
#define MAP 1	// packet group for sending data points
8 10
#define POINT_ODO 2
......
32 34
{
33 35
    /* initialize components and join the token ring */
34 36
    dragonfly_init(ALL_ON);
35
    odometry_init();
37
    odometry_init(); // breaks motors (interrupt-related?)
36 38
    orb_enable();
37 39
    wl_init();
38
    wl_set_channel(0xE);
40
    wl_set_channel(CHAN);
39 41
    wl_register_packet_group(&packetHandler);
40
    
42
   
43
    // initial control values
41 44
    velocity = 190;
42 45
    state = CNTL_STOP;
43

  
44
    int x, y; // use when sending odometry data
46
    
47
    // use when sending odometry data
48
    int x, y;
45 49
    double theta;
46 50

  
47 51
    int store[9];
......
49 53

  
50 54
    while(1)
51 55
    {
52
	wl_do();
53
	delay_ms(POLLING_INTERVAL);	/* until it's time to grab map data */
54
	x = odometry_dx(); // use when sending odometry data
55
	y = odometry_dy();
56
	theta = odometry_angle();
57
	store[0] = x;
58
	store[1] = y;
59
	*((double*)(store + 2)) = theta;
60
	store[4] = range_read_distance(IR1);	// IR1 range
61
	store[5] = range_read_distance(IR2);	// IR2 range
62
	store[6] = range_read_distance(IR3);	// IR3 range
63
	store[7] = range_read_distance(IR4);	// IR4 range
64
	store[8] = range_read_distance(IR5);	// IR5 range
65
	sprintf( buf, "%d %d %f %d %d %d %d %d\n", store[0], store[1],
66
	    *((double *)(store+2)), store[4], store[5], store[6],
67
	    store[7], store[8] );
68
	usb_puts(buf);
56
        wl_do();
57
        delay_ms(POLLING_INTERVAL);	// until it's time to grab map data
58
        x = odometry_dx(); // use when sending odometry data
59
        y = odometry_dy();
60
        theta = odometry_angle();
61
        store[0] = x;
62
        store[1] = y;
63
        *((double*)(store + 2)) = theta;
64
        store[4] = range_read_distance(IR1);	// IR1 range
65
        store[5] = range_read_distance(IR2);	// IR2 range
66
        store[6] = range_read_distance(IR3);	// IR3 range
67
        store[7] = range_read_distance(IR4);	// IR4 range
68
        store[8] = range_read_distance(IR5);	// IR5 range
69
        sprintf( buf, "%d %d %f %d %d %d %d %d\n", store[0], store[1],
70
                 *((double *)(store+2)), store[4], store[5], store[6],
71
                 store[7], store[8] );
72
        usb_puts(buf);
69 73

  
70
	send = (char*) store;
74
        send = (char*) store;
71 75

  
72
	wl_send_global_packet(MAP, POINT_ODO, send, 18, 0);
76
        wl_send_global_packet(MAP, POINT_ODO, send, 18, 0);
73 77
    }
74 78

  
75 79
    wl_terminate();
trunk/code/projects/mapping/server/test.c
4 4
#include "../../libwireless/lib/wireless.h"
5 5
#include <unistd.h>
6 6

  
7
#define CHAN 0xE
8

  
7 9
#define MAP 1   // packet group for receiving points
8 10
#define POINT_RAW 1 // packet type for map data points w/ raw encoder data
9 11
#define POINT_ODO 2 // packet type for map data points w/ odometry data
10 12

  
11 13
#define WL_CNTL_GROUP 4
12 14

  
13
#define CNTL_FORWARD 0
14
#define CNTL_BACK    1
15
#define CNTL_LEFT    2
16
#define CNTL_RIGHT   3
17
#define CNTL_LEFT_CURVE 4
18
#define CNTL_RIGHT_CURVE 5
19
#define CNTL_STOP    6
20
#define CNTL_VEL_UP  7
21
#define CNTL_VEL_DOWN 8
15
#define CNTL_FORWARD      0
16
#define CNTL_BACK         1
17
#define CNTL_LEFT         2
18
#define CNTL_RIGHT        3
19
#define CNTL_LEFT_CURVE   4
20
#define CNTL_RIGHT_CURVE  5
21
#define CNTL_STOP         6
22
#define CNTL_VEL_UP       7
23
#define CNTL_VEL_DOWN     8
22 24

  
23
void send_packet (char color, int dst_robot);
25
void send_packet (char type, int dst_robot);
24 26
void packet_receive(char type, int source, unsigned char* packet, int length);
25 27

  
26 28
PacketGroupHandler cntlHandler = {WL_CNTL_GROUP, NULL, NULL, NULL, NULL};
......
39 41
    int robot = atoi(argv[1]);
40 42

  
41 43
    printf("Beginning: robot=%d\r\n", robot);
42
	wl_init();
44
    wl_init();
43 45
	wl_set_com_port("/dev/ttyUSB1");
44
	wl_set_channel(0xE);
46
	wl_set_channel(CHAN);
45 47
	printf("Wireless initialized.\r\n");
46 48
	wl_register_packet_group(&cntlHandler);
47 49
	wl_register_packet_group(&receiveHandler);

Also available in: Unified diff