Project

General

Profile

Revision 915

Added by Chris Mar over 15 years ago

updated robot and server code to use odometry. having problems with theta because it is a double (4 bytes on robot, 8 bytes on server)

View differences:

robot_main.c
1 1 
#include <dragonfly_lib.h>
2 2 
#include <wireless.h>
3 3 
#include <encoders.h>
4 
//#include "odometry.h"
4 
#include "odometry.h"
5 5 

  		




  6
  
  
    
#define POLLING_INTERVAL 50	/* interval in ms to get sensor data */


  		




  
  6
  
    
#define POLLING_INTERVAL 200	/* interval in ms to get sensor data */


  		




  7
  7
  
    
#define MAP 1	// packet group for sending data points


  		




  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


  		




  ......




  17
  17
  
    
#define CNTL_LEFT_CURVE 4


  		




  18
  18
  
    
#define CNTL_RIGHT_CURVE 5


  		




  19
  19
  
    
#define CNTL_STOP    6


  		




  20
  
  
    
#define VEL_UP  7


  		




  21
  
  
    
#define VEL_DOWN 8


  		




  
  20
  
    
#define CNTL_VEL_UP  7


  		




  
  21
  
    
#define CNTL_VEL_DOWN 8


  		




  22
  22
  
    

  		




  23
  23
  
    
int state;


  		




  24
  24
  
    
int velocity;


  		




  ......




  34
  34
  
    
{


  		




  35
  35
  
    
    /* initialize components and join the token ring */


  		




  36
  36
  
    
    dragonfly_init(ALL_ON);


  		




  37
  
  
    
    range_init();


  		




  
  37
  
    
    //range_init();


  		




  
  38
  
    
    //encoders_init();


  		




  
  39
  
    
    odometry_init();


  		




  38
  40
  
    
    wl_init();


  		




  39
  
  
    
    encoders_init();


  		




  40
  41
  
    
    wl_set_channel(0xF);


  		




  41
  42
  
    
    wl_register_packet_group(&love_handle);


  		




  42
  43
  
    
    wl_register_packet_group(&packetHandler);


  		




  ......




  45
  46
  
    
    //wl_token_ring_register();


  		




  46
  47
  
    
    //wl_token_ring_join();


  		




  47
  48
  
    
    //run_around_init();


  		




  48
  
  
    
    //odometry_init();


  		




  49
  49
  
    

  		




  50
  50
  
    
    velocity = 160;


  		




  51
  51
  
    
    state = CNTL_STOP;


  		




  52
  52
  
    

  		




  53
  
  
    
    //int x, y; // use when sending odometry data


  		




  54
  
  
    
    //float theta;


  		




  
  53
  
    
    int x, y; // use when sending odometry data


  		




  
  54
  
    
    double theta;


  		




  55
  55
  
    

  		




  56
  
  
    
    int store[7];


  		




  
  56
  
    
    int store[9];


  		




  57
  57
  
    
    char *send;   // for transmission


  		




  58
  58
  
    

  		




  59
  59
  
    
    while(1)


  		




  ......




  61
  61
  
    
	wl_do();


  		




  62
  62
  
    
	//run_around_FSM();	/* traverse the environment and avoid obstacles ... */


  		




  63
  63
  
    
	delay_ms(POLLING_INTERVAL);	/* until it's time to grab map data */


  		




  64
  
  
    
	//x = odometry_dx(); // use when sending odometry data


  		




  65
  
  
    
	//y = odometry_dy();


  		




  66
  
  
    
	//theta = odometry_angle();


  		




  67
  
  
    
	store[0] = encoder_get_dx(LEFT);    // left encoder distance


  		




  68
  
  
    
	store[1] = encoder_get_dx(RIGHT);   // right encoder distance


  		




  69
  
  
    
	store[2] = range_read_distance(IR1);	// IR1 range


  		




  70
  
  
    
	store[3] = range_read_distance(IR2);	// IR2 range


  		




  71
  
  
    
	store[4] = range_read_distance(IR3);	// IR3 range


  		




  72
  
  
    
	store[5] = range_read_distance(IR4);	// IR4 range


  		




  73
  
  
    
	store[6] = range_read_distance(IR5);	// IR5 range


  		




  
  64
  
    
	x = odometry_dx(); // use when sending odometry data


  		




  
  65
  
    
	y = odometry_dy();


  		




  
  66
  
    
	theta = odometry_angle();


  		




  
  67
  
    
	//store[0] = encoder_get_dx(LEFT);    // left encoder distance


  		




  
  68
  
    
	//store[1] = encoder_get_dx(RIGHT);   // right encoder distance


  		




  
  69
  
    
	store[0] = x;


  		




  
  70
  
    
    store[1] = y;


  		




  
  71
  
    
    *((double*)((int*)store + 2)) = theta;


  		




  
  72
  
    
    store[4] = range_read_distance(IR1);	// IR1 range


  		




  
  73
  
    
	store[5] = range_read_distance(IR2);	// IR2 range


  		




  
  74
  
    
	store[6] = range_read_distance(IR3);	// IR3 range


  		




  
  75
  
    
	store[7] = range_read_distance(IR4);	// IR4 range


  		




  
  76
  
    
	store[8] = range_read_distance(IR5);	// IR5 range


  		




  
  77
  
    
    sprintf( buf, "%d %d %f %d %d %d %d %d\n", store[0], store[1],


  		




  
  78
  
    
          *((double *)((int*)store+2)), store[4], store[5], store[6],


  		




  
  79
  
    
          store[7], store[8] );


  		




  
  80
  
    
    usb_puts(buf);


  		




  74
  81
  
    

  		




  75
  82
  
    
	// convert to chars for transmission


  		




  76
  83
  
    
	//for (int i = 0; i < 7; i++)


  		




  ......




  82
  89
  
    
	send = (char*) store;


  		




  83
  90
  
    

  		




  84
  91
  
    
	// send point w/ raw encoder data


  		




  85
  
  
    
	wl_send_global_packet(MAP, POINT_RAW, send, 14, 0);


  		




  
  92
  
    
	// wl_send_global_packet(MAP, POINT_RAW, send, 14, 0);


  		




  86
  93
  
    

  		




  87
  94
  
    
	// send point w/ odometry data


  		




  88
  
  
    
	// wl_send_global_packet(MAP, POINT_ODO, send, 8, 0);


  		




  
  95
  
    
	wl_send_global_packet(MAP, POINT_ODO, send, 18, 0);


  		




  89
  96
  
    
	


  		




  90
  97
  
    
	// send raw encoder data point over serial as a string


  		




  91
  98
  
    
	// send[7] = 0;


  		




  92
  99
  
    
	// usb_puts(send);	// this is probably ugly, but I don't know any better


  		




  93
  100
  
    
	


  		




  94
  101
  
    
	// send raw encoder data point over serial as integers


  		




  95
  
  
    
	for (int i = 0; i < 7; i++)


  		




  
  102
  
    
	/*for (int i = 0; i < 7; i++)


  		




  96
  103
  
    
	{


  		




  97
  104
  
    
	    usb_puti(store[i]);


  		




  98
  105
  
    
	    usb_putc(' ');


  		




  99
  106
  
    
	}


  		




  100
  
  
    
	usb_putc('\n');


  		




  101
  
  
    
	usb_puti(range_read_distance(IR1));


  		




  102
  
  
    
	usb_putc('\n');


  		




  
  107
  
    
    usb_putc('\n');*/


  		




  103
  108
  
    
    }


  		




  104
  109
  
    

  		




  105
  110
  
    
    wl_terminate();


  		




  ......




  131
  136
  
    
        case CNTL_STOP:


  		




  132
  137
  
    
            state = CNTL_STOP;


  		




  133
  138
  
    
            break;


  		




  134
  
  
    
        case VEL_UP:


  		




  
  139
  
    
        case CNTL_VEL_UP:


  		




  135
  140
  
    
            if (velocity > 250)


  		




  136
  141
  
    
                velocity = 250;


  		




  137
  142
  
    
            else


  		




  138
  143
  
    
                velocity += 5;


  		




  139
  144
  
    
            break;


  		




  140
  
  
    
        case VEL_DOWN:


  		




  
  145
  
    
        case CNTL_VEL_DOWN:


  		




  141
  146
  
    
            velocity -= 5;


  		




  142
  147
  
    
            if (velocity < 150)


  		




  143
  148
  
    
                velocity = 150;


  		












Also available in:  Unified diff