aigamozu
/
program1
test1
main.cpp
- Committer:
- m5171135
- Date:
- 2014-05-08
- Revision:
- 0:47edd2ca15c6
- Child:
- 1:a961c533bc91
- Child:
- 2:f8bfd160f174
File content as of revision 0:47edd2ca15c6:
#include "mbed.h" #define BUFFER_SIZE 512 //pin set //Serial,I2C I2C i2c(p9, p10); // sda, scl Serial pc(USBTX, USBRX); // tx, rx Serial gps(p28, p27); Serial xbee(p13,p14); //motor DigitalOut motor1_A(p21); DigitalOut motor1_B(p22); PwmOut motor1_pwm(p23); DigitalOut motor2_A(p24); DigitalOut motor2_B(p25); PwmOut motor2_pwm(p26); //encorder InterruptIn encoder1_A(p15); InterruptIn encoder1_B(p16); Ticker axis; //I2C address 9-axis const int gyro_addr = 0xD0; const int acc_addr = 0xA6; //Each value char gyro_head[1]; char read[8]; short int gyro_x=0; short int gyro_y=0; short int gyro_z=0; short int tempr=0; char acc_head[1]; char acc_buf[6]; short int acc_x = 0; short int acc_y = 0; short int acc_z = 0; float target_palse = 0.0; float pwm; long encorder_count = 0; //GPS typedef unsigned char UBYTE; typedef unsigned int UWORD; typedef struct { UWORD cap; UWORD wp; UWORD last_head_rp; UWORD head_rp; UWORD rp; UBYTE dat[BUFFER_SIZE]; } RINGP; RINGP gps_buf; UBYTE get_ring(void){ UBYTE result ; /* load data */ result = gps_buf.dat[gps_buf.rp] ; /* update pointer */ gps_buf.rp += 1 ; if ( gps_buf.rp == BUFFER_SIZE ) { gps_buf.rp = 0 ; } /* capacity decrement */ gps_buf.cap -= 1 ; return result ; } void put_ring(UBYTE x){ /* store data */ gps_buf.dat[gps_buf.wp] = x ; /* update pointer */ gps_buf.wp += 1 ; if ( gps_buf.wp == BUFFER_SIZE ) { gps_buf.wp = 0 ; } /* capacity increment */ gps_buf.cap += 1 ; } void init_ring(void){ gps_buf.cap=0; gps_buf.wp=0; gps_buf.last_head_rp=0; gps_buf.head_rp=0; gps_buf.rp=0; } void axisRenovation(){ //gyro gyro_head[0] = 0x1B; i2c.write(gyro_addr, gyro_head, 1); i2c.read(gyro_addr, read, 8); tempr=(read[0] << 8) + read[1]; gyro_x=(read[2] << 8) + read[3]; gyro_y=(read[4] << 8) + read[5]; gyro_z=(read[6] << 8) + read[7]; //acc acc_head[0] = 0x32; i2c.write(acc_addr,acc_head,1); i2c.read(acc_addr, acc_buf, 6); acc_x = (acc_buf[1] << 8) + acc_buf[0]; acc_y = (acc_buf[3] << 8) + acc_buf[2]; acc_z = (acc_buf[5] << 8) + acc_buf[4]; xbee.printf("%d, %d, %d ,", gyro_x, gyro_y, gyro_z); //xbee.printf("%d, %d, %d", acc_x, acc_y, acc_z); xbee.printf("%ld,%lf,%lf\n",encorder_count,target_palse,pwm); //feedback pwm = 0.01 * (target_palse - encorder_count/11264.0) + motor1_pwm; motor1_pwm = pwm; encorder_count = 0; char val; gps_buf.rp = gps_buf.last_head_rp; while(1){ val = get_ring(); xbee.printf("%c",val); if(val == 0x0a) break; } } void gps_rx(){ char val = gps.getc(); put_ring(val); if(val == '$'){ gps_buf.last_head_rp = gps_buf.head_rp; gps_buf.head_rp = gps_buf.wp; } } void xbee_rx(){ char val = xbee.getc(); if(val == 'a') { motor1_A = 1; motor1_B = 0; target_palse = 5.0; //motor2_A = 1; //motor2_B = 0; } else if(val == 's'){ motor1_A = 0; motor1_B = 0; target_palse = 0.0; //motor2_A = 0; //motor2_B = 0; } else if(val == 'd'){ target_palse = target_palse + 0.1; } else if(val == 'f'){ target_palse = target_palse - 0.1; } } void send_GPS_command(){ char gps_command1[] = "$PMTK220,500*2B"; char gps_command2[] = "$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29"; char CR = 0x0d; char LF = 0x0a; gps.printf("%s%c%c",gps_command1,CR,LF); gps.printf("%s%c%c",gps_command2,CR,LF); } void flip(){ encorder_count++; } int main() { //start up time wait(1); //set i2c frequency to 400 KHz i2c.frequency(400000); //set pc frequency to 57600bps pc.baud(57600); //set xbee frequency to 57600bps xbee.baud(57600); //GPS setting send_GPS_command(); init_ring(); gps.attach(gps_rx,Serial::RxIrq); xbee.attach(xbee_rx,Serial::RxIrq); wait(5); //Encorder Innterrapt Setting encoder1_A.rise(&flip); //gyro_registor Setting char PWR_M[2]={0x3E,0x80}; i2c.write(gyro_addr, PWR_M, 2); // Send command string char SMPL[2]={0x15,0x00}; i2c.write(gyro_addr, SMPL, 2); // Send command string char DLPF[2]={0x16,0x18}; i2c.write(gyro_addr, DLPF, 2); // Send command string char INT_C[2]={0x17,0x05}; i2c.write(gyro_addr, INT_C, 2); // Send commanad string char PWR_M2[2]={0x3E,0x00}; i2c.write(gyro_addr, PWR_M2, 2); // Send command string wait(1); pc.printf("start\n"); axis.attach(&axisRenovation, 0.1); motor1_pwm = 0.0; //motor2_pwm = 0.3; while (1) { } }