aigamozu / Mbed 2 deprecated program1

test1

Dependencies:   mbed XBee

main.cpp@1:a961c533bc91, 2014-05-08 (annotated)

Committer:
m5171135
Date:
Thu May 08 12:56:19 2014 +0000
Revision:
1:a961c533bc91
Parent:
0:47edd2ca15c6
Child:
5:e1b1b9a43ccb
ver 1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m5171135 0:47edd2ca15c6 1 #include "mbed.h"
m5171135 0:47edd2ca15c6 2 #define BUFFER_SIZE 512
m5171135 0:47edd2ca15c6 3
m5171135 0:47edd2ca15c6 4 //pin set
m5171135 0:47edd2ca15c6 5 //Serial,I2C
m5171135 1:a961c533bc91 6 I2C i2c(p9, p10); //sda, scl,MFU
m5171135 1:a961c533bc91 7 Serial pc(USBTX, USBRX); //tx, rx
m5171135 1:a961c533bc91 8 Serial gps(p28, p27); //tx, rx
m5171135 1:a961c533bc91 9 Serial xbee(p13,p14); //tx,rx
m5171135 0:47edd2ca15c6 10
m5171135 0:47edd2ca15c6 11 //motor
m5171135 0:47edd2ca15c6 12 DigitalOut motor1_A(p21);
m5171135 0:47edd2ca15c6 13 DigitalOut motor1_B(p22);
m5171135 0:47edd2ca15c6 14 PwmOut motor1_pwm(p23);
m5171135 0:47edd2ca15c6 15
m5171135 0:47edd2ca15c6 16 DigitalOut motor2_A(p24);
m5171135 0:47edd2ca15c6 17 DigitalOut motor2_B(p25);
m5171135 0:47edd2ca15c6 18 PwmOut motor2_pwm(p26);
m5171135 0:47edd2ca15c6 19
m5171135 0:47edd2ca15c6 20 //encorder
m5171135 0:47edd2ca15c6 21 InterruptIn encoder1_A(p15);
m5171135 0:47edd2ca15c6 22 InterruptIn encoder1_B(p16);
m5171135 0:47edd2ca15c6 23
m5171135 0:47edd2ca15c6 24 Ticker axis;
m5171135 0:47edd2ca15c6 25
m5171135 0:47edd2ca15c6 26 //I2C address 9-axis
m5171135 0:47edd2ca15c6 27 const int gyro_addr = 0xD0;
m5171135 0:47edd2ca15c6 28 const int acc_addr = 0xA6;
m5171135 0:47edd2ca15c6 29
m5171135 0:47edd2ca15c6 30 //Each value
m5171135 0:47edd2ca15c6 31 char gyro_head[1];
m5171135 0:47edd2ca15c6 32 char read[8];
m5171135 0:47edd2ca15c6 33 short int gyro_x=0;
m5171135 0:47edd2ca15c6 34 short int gyro_y=0;
m5171135 0:47edd2ca15c6 35 short int gyro_z=0;
m5171135 0:47edd2ca15c6 36 short int tempr=0;
m5171135 0:47edd2ca15c6 37
m5171135 0:47edd2ca15c6 38 char acc_head[1];
m5171135 0:47edd2ca15c6 39 char acc_buf[6];
m5171135 0:47edd2ca15c6 40 short int acc_x = 0;
m5171135 0:47edd2ca15c6 41 short int acc_y = 0;
m5171135 0:47edd2ca15c6 42 short int acc_z = 0;
m5171135 0:47edd2ca15c6 43
m5171135 1:a961c533bc91 44 //motor_speed_feed_back
m5171135 0:47edd2ca15c6 45 float target_palse = 0.0;
m5171135 0:47edd2ca15c6 46 float pwm;
m5171135 0:47edd2ca15c6 47 long encorder_count = 0;
m5171135 0:47edd2ca15c6 48
m5171135 0:47edd2ca15c6 49 //GPS
m5171135 1:a961c533bc91 50 //Ring Buffer
m5171135 0:47edd2ca15c6 51 typedef unsigned char UBYTE;
m5171135 0:47edd2ca15c6 52 typedef unsigned int UWORD;
m5171135 0:47edd2ca15c6 53
m5171135 0:47edd2ca15c6 54 typedef struct {
m5171135 0:47edd2ca15c6 55 UWORD cap;
m5171135 0:47edd2ca15c6 56 UWORD wp;
m5171135 0:47edd2ca15c6 57 UWORD last_head_rp;
m5171135 0:47edd2ca15c6 58 UWORD head_rp;
m5171135 0:47edd2ca15c6 59 UWORD rp;
m5171135 0:47edd2ca15c6 60 UBYTE dat[BUFFER_SIZE];
m5171135 0:47edd2ca15c6 61 } RINGP;
m5171135 0:47edd2ca15c6 62
m5171135 0:47edd2ca15c6 63 RINGP gps_buf;
m5171135 0:47edd2ca15c6 64
m5171135 0:47edd2ca15c6 65 UBYTE get_ring(void){
m5171135 0:47edd2ca15c6 66 UBYTE result ;
m5171135 0:47edd2ca15c6 67 /* load data */
m5171135 0:47edd2ca15c6 68 result = gps_buf.dat[gps_buf.rp] ;
m5171135 0:47edd2ca15c6 69 /* update pointer */
m5171135 0:47edd2ca15c6 70 gps_buf.rp += 1 ;
m5171135 0:47edd2ca15c6 71 if ( gps_buf.rp == BUFFER_SIZE ) { gps_buf.rp = 0 ; }
m5171135 0:47edd2ca15c6 72 /* capacity decrement */
m5171135 0:47edd2ca15c6 73 gps_buf.cap -= 1 ;
m5171135 0:47edd2ca15c6 74
m5171135 0:47edd2ca15c6 75 return result ;
m5171135 0:47edd2ca15c6 76 }
m5171135 0:47edd2ca15c6 77
m5171135 0:47edd2ca15c6 78 void put_ring(UBYTE x){
m5171135 0:47edd2ca15c6 79 /* store data */
m5171135 0:47edd2ca15c6 80 gps_buf.dat[gps_buf.wp] = x ;
m5171135 0:47edd2ca15c6 81 /* update pointer */
m5171135 0:47edd2ca15c6 82 gps_buf.wp += 1 ;
m5171135 0:47edd2ca15c6 83 if ( gps_buf.wp == BUFFER_SIZE ) { gps_buf.wp = 0 ; }
m5171135 0:47edd2ca15c6 84 /* capacity increment */
m5171135 0:47edd2ca15c6 85 gps_buf.cap += 1 ;
m5171135 0:47edd2ca15c6 86 }
m5171135 0:47edd2ca15c6 87
m5171135 0:47edd2ca15c6 88 void init_ring(void){
m5171135 0:47edd2ca15c6 89 gps_buf.cap=0;
m5171135 0:47edd2ca15c6 90 gps_buf.wp=0;
m5171135 0:47edd2ca15c6 91 gps_buf.last_head_rp=0;
m5171135 0:47edd2ca15c6 92 gps_buf.head_rp=0;
m5171135 0:47edd2ca15c6 93 gps_buf.rp=0;
m5171135 0:47edd2ca15c6 94 }
m5171135 0:47edd2ca15c6 95
m5171135 0:47edd2ca15c6 96 void axisRenovation(){
m5171135 0:47edd2ca15c6 97 //gyro
m5171135 0:47edd2ca15c6 98 gyro_head[0] = 0x1B;
m5171135 0:47edd2ca15c6 99 i2c.write(gyro_addr, gyro_head, 1);
m5171135 0:47edd2ca15c6 100 i2c.read(gyro_addr, read, 8);
m5171135 0:47edd2ca15c6 101
m5171135 0:47edd2ca15c6 102 tempr=(read[0] << 8) + read[1];
m5171135 0:47edd2ca15c6 103 gyro_x=(read[2] << 8) + read[3];
m5171135 0:47edd2ca15c6 104 gyro_y=(read[4] << 8) + read[5];
m5171135 0:47edd2ca15c6 105 gyro_z=(read[6] << 8) + read[7];
m5171135 0:47edd2ca15c6 106
m5171135 0:47edd2ca15c6 107 //acc
m5171135 0:47edd2ca15c6 108 acc_head[0] = 0x32;
m5171135 0:47edd2ca15c6 109 i2c.write(acc_addr,acc_head,1);
m5171135 0:47edd2ca15c6 110 i2c.read(acc_addr, acc_buf, 6);
m5171135 0:47edd2ca15c6 111
m5171135 0:47edd2ca15c6 112 acc_x = (acc_buf[1] << 8) + acc_buf[0];
m5171135 0:47edd2ca15c6 113 acc_y = (acc_buf[3] << 8) + acc_buf[2];
m5171135 0:47edd2ca15c6 114 acc_z = (acc_buf[5] << 8) + acc_buf[4];
m5171135 0:47edd2ca15c6 115
m5171135 1:a961c533bc91 116 //xbee.printf("%d, %d, %d ,", gyro_x, gyro_y, gyro_z);
m5171135 0:47edd2ca15c6 117 //xbee.printf("%d, %d, %d", acc_x, acc_y, acc_z);
m5171135 1:a961c533bc91 118 //xbee.printf("%ld,%lf,%lf\n",encorder_count,target_palse,pwm);
m5171135 0:47edd2ca15c6 119
m5171135 0:47edd2ca15c6 120
m5171135 0:47edd2ca15c6 121 //feedback
m5171135 1:a961c533bc91 122 //pwm = 0.01 * (target_palse - encorder_count/11264.0) + motor1_pwm;
m5171135 1:a961c533bc91 123 //motor1_pwm = pwm;
m5171135 1:a961c533bc91 124 //encorder_count = 0;
m5171135 0:47edd2ca15c6 125
m5171135 1:a961c533bc91 126 //Output GPS data -> xbee
m5171135 0:47edd2ca15c6 127 char val;
m5171135 0:47edd2ca15c6 128 gps_buf.rp = gps_buf.last_head_rp;
m5171135 0:47edd2ca15c6 129 while(1){
m5171135 0:47edd2ca15c6 130 val = get_ring();
m5171135 0:47edd2ca15c6 131 xbee.printf("%c",val);
m5171135 0:47edd2ca15c6 132 if(val == 0x0a) break;
m5171135 0:47edd2ca15c6 133 }
m5171135 1:a961c533bc91 134
m5171135 0:47edd2ca15c6 135 }
m5171135 0:47edd2ca15c6 136
m5171135 0:47edd2ca15c6 137 void gps_rx(){
m5171135 0:47edd2ca15c6 138 char val = gps.getc();
m5171135 0:47edd2ca15c6 139 put_ring(val);
m5171135 0:47edd2ca15c6 140 if(val == '$'){
m5171135 0:47edd2ca15c6 141 gps_buf.last_head_rp = gps_buf.head_rp;
m5171135 0:47edd2ca15c6 142 gps_buf.head_rp = gps_buf.wp;
m5171135 0:47edd2ca15c6 143 }
m5171135 0:47edd2ca15c6 144 }
m5171135 0:47edd2ca15c6 145
m5171135 0:47edd2ca15c6 146 void xbee_rx(){
m5171135 0:47edd2ca15c6 147 char val = xbee.getc();
m5171135 0:47edd2ca15c6 148 if(val == 'a') {
m5171135 0:47edd2ca15c6 149 motor1_A = 1;
m5171135 0:47edd2ca15c6 150 motor1_B = 0;
m5171135 1:a961c533bc91 151 motor1_pwm = 0.5;
m5171135 0:47edd2ca15c6 152 //motor2_A = 1;
m5171135 0:47edd2ca15c6 153 //motor2_B = 0;
m5171135 0:47edd2ca15c6 154 }
m5171135 0:47edd2ca15c6 155 else if(val == 's'){
m5171135 0:47edd2ca15c6 156 motor1_A = 0;
m5171135 0:47edd2ca15c6 157 motor1_B = 0;
m5171135 1:a961c533bc91 158 motor1_pwm = 0.0;
m5171135 0:47edd2ca15c6 159 //motor2_A = 0;
m5171135 0:47edd2ca15c6 160 //motor2_B = 0;
m5171135 0:47edd2ca15c6 161 }
m5171135 0:47edd2ca15c6 162 else if(val == 'd'){
m5171135 0:47edd2ca15c6 163 target_palse = target_palse + 0.1;
m5171135 0:47edd2ca15c6 164 }
m5171135 0:47edd2ca15c6 165 else if(val == 'f'){
m5171135 0:47edd2ca15c6 166 target_palse = target_palse - 0.1;
m5171135 0:47edd2ca15c6 167 }
m5171135 0:47edd2ca15c6 168
m5171135 0:47edd2ca15c6 169
m5171135 0:47edd2ca15c6 170 }
m5171135 0:47edd2ca15c6 171
m5171135 0:47edd2ca15c6 172 void send_GPS_command(){
m5171135 0:47edd2ca15c6 173 char gps_command1[] = "$PMTK220,500*2B";
m5171135 0:47edd2ca15c6 174 char gps_command2[] = "$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29";
m5171135 0:47edd2ca15c6 175
m5171135 0:47edd2ca15c6 176 char CR = 0x0d;
m5171135 0:47edd2ca15c6 177 char LF = 0x0a;
m5171135 0:47edd2ca15c6 178
m5171135 0:47edd2ca15c6 179 gps.printf("%s%c%c",gps_command1,CR,LF);
m5171135 0:47edd2ca15c6 180 gps.printf("%s%c%c",gps_command2,CR,LF);
m5171135 0:47edd2ca15c6 181
m5171135 0:47edd2ca15c6 182 }
m5171135 0:47edd2ca15c6 183
m5171135 0:47edd2ca15c6 184
m5171135 0:47edd2ca15c6 185 void flip(){
m5171135 0:47edd2ca15c6 186 encorder_count++;
m5171135 0:47edd2ca15c6 187 }
m5171135 0:47edd2ca15c6 188
m5171135 0:47edd2ca15c6 189
m5171135 0:47edd2ca15c6 190 int main() {
m5171135 0:47edd2ca15c6 191 //start up time
m5171135 0:47edd2ca15c6 192 wait(1);
m5171135 0:47edd2ca15c6 193 //set i2c frequency to 400 KHz
m5171135 0:47edd2ca15c6 194 i2c.frequency(400000);
m5171135 0:47edd2ca15c6 195 //set pc frequency to 57600bps
m5171135 0:47edd2ca15c6 196 pc.baud(57600);
m5171135 0:47edd2ca15c6 197 //set xbee frequency to 57600bps
m5171135 0:47edd2ca15c6 198 xbee.baud(57600);
m5171135 0:47edd2ca15c6 199
m5171135 0:47edd2ca15c6 200 //GPS setting
m5171135 0:47edd2ca15c6 201 send_GPS_command();
m5171135 0:47edd2ca15c6 202 init_ring();
m5171135 0:47edd2ca15c6 203 gps.attach(gps_rx,Serial::RxIrq);
m5171135 0:47edd2ca15c6 204 xbee.attach(xbee_rx,Serial::RxIrq);
m5171135 0:47edd2ca15c6 205 wait(5);
m5171135 0:47edd2ca15c6 206
m5171135 0:47edd2ca15c6 207 //Encorder Innterrapt Setting
m5171135 1:a961c533bc91 208 //encoder1_A.rise(&flip);
m5171135 0:47edd2ca15c6 209
m5171135 0:47edd2ca15c6 210 //gyro_registor Setting
m5171135 0:47edd2ca15c6 211 char PWR_M[2]={0x3E,0x80};
m5171135 0:47edd2ca15c6 212 i2c.write(gyro_addr, PWR_M, 2); // Send command string
m5171135 0:47edd2ca15c6 213 char SMPL[2]={0x15,0x00};
m5171135 0:47edd2ca15c6 214 i2c.write(gyro_addr, SMPL, 2); // Send command string
m5171135 0:47edd2ca15c6 215 char DLPF[2]={0x16,0x18};
m5171135 0:47edd2ca15c6 216 i2c.write(gyro_addr, DLPF, 2); // Send command string
m5171135 0:47edd2ca15c6 217 char INT_C[2]={0x17,0x05};
m5171135 0:47edd2ca15c6 218 i2c.write(gyro_addr, INT_C, 2); // Send commanad string
m5171135 0:47edd2ca15c6 219 char PWR_M2[2]={0x3E,0x00};
m5171135 0:47edd2ca15c6 220 i2c.write(gyro_addr, PWR_M2, 2); // Send command string
m5171135 1:a961c533bc91 221 wait(1);
m5171135 0:47edd2ca15c6 222
m5171135 1:a961c533bc91 223 //interrupt start
m5171135 0:47edd2ca15c6 224 axis.attach(&axisRenovation, 0.1);
m5171135 0:47edd2ca15c6 225 motor1_pwm = 0.0;
m5171135 0:47edd2ca15c6 226
m5171135 0:47edd2ca15c6 227 while (1) {
m5171135 0:47edd2ca15c6 228 }
m5171135 0:47edd2ca15c6 229 }