aigamozu / Mbed 2 deprecated program1

test1

Dependencies:   mbed XBee

main.cpp@5:e1b1b9a43ccb, 2014-05-11 (annotated)

Committer:
m5171135
Date:
Sun May 11 09:08:50 2014 +0000
Revision:
5:e1b1b9a43ccb
Parent:
1:a961c533bc91 
ver. xbeeAPImode

Who changed what in which revision?

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