aigamozu / Mbed 2 deprecated program1

test1

Dependencies:   mbed XBee

main.cpp@3:6c27096e87e7, 2014-05-08 (annotated)

Committer:
s1200145
Date:
Thu May 08 13:06:17 2014 +0000
Revision:
3:6c27096e87e7
Parent:
2:f8bfd160f174 
test taku

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
s1200145 2:f8bfd160f174 3 //hello
s1200145 3:6c27096e87e7 4 //pin setdd
m5171135 0:47edd2ca15c6 5 //Serial,I2C
m5171135 0:47edd2ca15c6 6 I2C i2c(p9, p10); // sda, scl
m5171135 0:47edd2ca15c6 7 Serial pc(USBTX, USBRX); // tx, rx
m5171135 0:47edd2ca15c6 8 Serial gps(p28, p27);
m5171135 0:47edd2ca15c6 9 Serial xbee(p13,p14);
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
m5171135 0:47edd2ca15c6 34 short int gyro_x=0;
m5171135 0:47edd2ca15c6 35 short int gyro_y=0;
m5171135 0:47edd2ca15c6 36 short int gyro_z=0;
m5171135 0:47edd2ca15c6 37 short int tempr=0;
m5171135 0:47edd2ca15c6 38
m5171135 0:47edd2ca15c6 39 char acc_head[1];
m5171135 0:47edd2ca15c6 40 char acc_buf[6];
m5171135 0:47edd2ca15c6 41 short int acc_x = 0;
m5171135 0:47edd2ca15c6 42 short int acc_y = 0;
m5171135 0:47edd2ca15c6 43 short int acc_z = 0;
m5171135 0:47edd2ca15c6 44
m5171135 0:47edd2ca15c6 45 float target_palse = 0.0;
m5171135 0:47edd2ca15c6 46 float pwm;
m5171135 0:47edd2ca15c6 47
m5171135 0:47edd2ca15c6 48 long encorder_count = 0;
m5171135 0:47edd2ca15c6 49
m5171135 0:47edd2ca15c6 50 //GPS
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
m5171135 0:47edd2ca15c6 66 UBYTE get_ring(void){
m5171135 0:47edd2ca15c6 67 UBYTE result ;
m5171135 0:47edd2ca15c6 68 /* load data */
m5171135 0:47edd2ca15c6 69 result = gps_buf.dat[gps_buf.rp] ;
m5171135 0:47edd2ca15c6 70 /* update pointer */
m5171135 0:47edd2ca15c6 71 gps_buf.rp += 1 ;
m5171135 0:47edd2ca15c6 72 if ( gps_buf.rp == BUFFER_SIZE ) { gps_buf.rp = 0 ; }
m5171135 0:47edd2ca15c6 73 /* capacity decrement */
m5171135 0:47edd2ca15c6 74 gps_buf.cap -= 1 ;
m5171135 0:47edd2ca15c6 75
m5171135 0:47edd2ca15c6 76 return result ;
m5171135 0:47edd2ca15c6 77 }
m5171135 0:47edd2ca15c6 78
m5171135 0:47edd2ca15c6 79 void put_ring(UBYTE x){
m5171135 0:47edd2ca15c6 80 /* store data */
m5171135 0:47edd2ca15c6 81 gps_buf.dat[gps_buf.wp] = x ;
m5171135 0:47edd2ca15c6 82 /* update pointer */
m5171135 0:47edd2ca15c6 83 gps_buf.wp += 1 ;
m5171135 0:47edd2ca15c6 84 if ( gps_buf.wp == BUFFER_SIZE ) { gps_buf.wp = 0 ; }
m5171135 0:47edd2ca15c6 85 /* capacity increment */
m5171135 0:47edd2ca15c6 86 gps_buf.cap += 1 ;
m5171135 0:47edd2ca15c6 87 }
m5171135 0:47edd2ca15c6 88
m5171135 0:47edd2ca15c6 89 void init_ring(void){
m5171135 0:47edd2ca15c6 90 gps_buf.cap=0;
m5171135 0:47edd2ca15c6 91 gps_buf.wp=0;
m5171135 0:47edd2ca15c6 92 gps_buf.last_head_rp=0;
m5171135 0:47edd2ca15c6 93 gps_buf.head_rp=0;
m5171135 0:47edd2ca15c6 94 gps_buf.rp=0;
m5171135 0:47edd2ca15c6 95 }
m5171135 0:47edd2ca15c6 96
m5171135 0:47edd2ca15c6 97 void axisRenovation(){
m5171135 0:47edd2ca15c6 98 //gyro
m5171135 0:47edd2ca15c6 99 gyro_head[0] = 0x1B;
m5171135 0:47edd2ca15c6 100 i2c.write(gyro_addr, gyro_head, 1);
m5171135 0:47edd2ca15c6 101 i2c.read(gyro_addr, read, 8);
m5171135 0:47edd2ca15c6 102
m5171135 0:47edd2ca15c6 103 tempr=(read[0] << 8) + read[1];
m5171135 0:47edd2ca15c6 104 gyro_x=(read[2] << 8) + read[3];
m5171135 0:47edd2ca15c6 105 gyro_y=(read[4] << 8) + read[5];
m5171135 0:47edd2ca15c6 106 gyro_z=(read[6] << 8) + read[7];
m5171135 0:47edd2ca15c6 107
m5171135 0:47edd2ca15c6 108 //acc
m5171135 0:47edd2ca15c6 109 acc_head[0] = 0x32;
m5171135 0:47edd2ca15c6 110 i2c.write(acc_addr,acc_head,1);
m5171135 0:47edd2ca15c6 111 i2c.read(acc_addr, acc_buf, 6);
m5171135 0:47edd2ca15c6 112
m5171135 0:47edd2ca15c6 113 acc_x = (acc_buf[1] << 8) + acc_buf[0];
m5171135 0:47edd2ca15c6 114 acc_y = (acc_buf[3] << 8) + acc_buf[2];
m5171135 0:47edd2ca15c6 115 acc_z = (acc_buf[5] << 8) + acc_buf[4];
m5171135 0:47edd2ca15c6 116
m5171135 0:47edd2ca15c6 117 xbee.printf("%d, %d, %d ,", gyro_x, gyro_y, gyro_z);
m5171135 0:47edd2ca15c6 118 //xbee.printf("%d, %d, %d", acc_x, acc_y, acc_z);
m5171135 0:47edd2ca15c6 119 xbee.printf("%ld,%lf,%lf\n",encorder_count,target_palse,pwm);
m5171135 0:47edd2ca15c6 120
m5171135 0:47edd2ca15c6 121
m5171135 0:47edd2ca15c6 122 //feedback
m5171135 0:47edd2ca15c6 123 pwm = 0.01 * (target_palse - encorder_count/11264.0) + motor1_pwm;
m5171135 0:47edd2ca15c6 124 motor1_pwm = pwm;
m5171135 0:47edd2ca15c6 125 encorder_count = 0;
m5171135 0:47edd2ca15c6 126
m5171135 0:47edd2ca15c6 127
m5171135 0:47edd2ca15c6 128 char val;
m5171135 0:47edd2ca15c6 129 gps_buf.rp = gps_buf.last_head_rp;
m5171135 0:47edd2ca15c6 130
m5171135 0:47edd2ca15c6 131 while(1){
m5171135 0:47edd2ca15c6 132 val = get_ring();
m5171135 0:47edd2ca15c6 133 xbee.printf("%c",val);
m5171135 0:47edd2ca15c6 134 if(val == 0x0a) break;
m5171135 0:47edd2ca15c6 135 }
m5171135 0:47edd2ca15c6 136 }
m5171135 0:47edd2ca15c6 137
m5171135 0:47edd2ca15c6 138 void gps_rx(){
m5171135 0:47edd2ca15c6 139 char val = gps.getc();
m5171135 0:47edd2ca15c6 140 put_ring(val);
m5171135 0:47edd2ca15c6 141 if(val == '$'){
m5171135 0:47edd2ca15c6 142 gps_buf.last_head_rp = gps_buf.head_rp;
m5171135 0:47edd2ca15c6 143 gps_buf.head_rp = gps_buf.wp;
m5171135 0:47edd2ca15c6 144 }
m5171135 0:47edd2ca15c6 145 }
m5171135 0:47edd2ca15c6 146
m5171135 0:47edd2ca15c6 147 void xbee_rx(){
m5171135 0:47edd2ca15c6 148 char val = xbee.getc();
m5171135 0:47edd2ca15c6 149
m5171135 0:47edd2ca15c6 150 if(val == 'a') {
m5171135 0:47edd2ca15c6 151 motor1_A = 1;
m5171135 0:47edd2ca15c6 152 motor1_B = 0;
m5171135 0:47edd2ca15c6 153 target_palse = 5.0;
m5171135 0:47edd2ca15c6 154 //motor2_A = 1;
m5171135 0:47edd2ca15c6 155 //motor2_B = 0;
m5171135 0:47edd2ca15c6 156 }
m5171135 0:47edd2ca15c6 157 else if(val == 's'){
m5171135 0:47edd2ca15c6 158 motor1_A = 0;
m5171135 0:47edd2ca15c6 159 motor1_B = 0;
m5171135 0:47edd2ca15c6 160 target_palse = 0.0;
m5171135 0:47edd2ca15c6 161
m5171135 0:47edd2ca15c6 162 //motor2_A = 0;
m5171135 0:47edd2ca15c6 163 //motor2_B = 0;
m5171135 0:47edd2ca15c6 164 }
m5171135 0:47edd2ca15c6 165 else if(val == 'd'){
m5171135 0:47edd2ca15c6 166 target_palse = target_palse + 0.1;
m5171135 0:47edd2ca15c6 167 }
m5171135 0:47edd2ca15c6 168 else if(val == 'f'){
m5171135 0:47edd2ca15c6 169 target_palse = target_palse - 0.1;
m5171135 0:47edd2ca15c6 170 }
m5171135 0:47edd2ca15c6 171
m5171135 0:47edd2ca15c6 172
m5171135 0:47edd2ca15c6 173 }
m5171135 0:47edd2ca15c6 174
m5171135 0:47edd2ca15c6 175 void send_GPS_command(){
m5171135 0:47edd2ca15c6 176 char gps_command1[] = "$PMTK220,500*2B";
m5171135 0:47edd2ca15c6 177 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 178
m5171135 0:47edd2ca15c6 179 char CR = 0x0d;
m5171135 0:47edd2ca15c6 180 char LF = 0x0a;
m5171135 0:47edd2ca15c6 181
m5171135 0:47edd2ca15c6 182 gps.printf("%s%c%c",gps_command1,CR,LF);
m5171135 0:47edd2ca15c6 183 gps.printf("%s%c%c",gps_command2,CR,LF);
m5171135 0:47edd2ca15c6 184
m5171135 0:47edd2ca15c6 185 }
m5171135 0:47edd2ca15c6 186
m5171135 0:47edd2ca15c6 187
m5171135 0:47edd2ca15c6 188 void flip(){
m5171135 0:47edd2ca15c6 189 encorder_count++;
m5171135 0:47edd2ca15c6 190 }
m5171135 0:47edd2ca15c6 191
m5171135 0:47edd2ca15c6 192
m5171135 0:47edd2ca15c6 193 int main() {
m5171135 0:47edd2ca15c6 194 //start up time
m5171135 0:47edd2ca15c6 195 wait(1);
m5171135 0:47edd2ca15c6 196 //set i2c frequency to 400 KHz
m5171135 0:47edd2ca15c6 197 i2c.frequency(400000);
m5171135 0:47edd2ca15c6 198 //set pc frequency to 57600bps
m5171135 0:47edd2ca15c6 199 pc.baud(57600);
m5171135 0:47edd2ca15c6 200 //set xbee frequency to 57600bps
m5171135 0:47edd2ca15c6 201 xbee.baud(57600);
m5171135 0:47edd2ca15c6 202
m5171135 0:47edd2ca15c6 203 //GPS setting
m5171135 0:47edd2ca15c6 204 send_GPS_command();
m5171135 0:47edd2ca15c6 205 init_ring();
m5171135 0:47edd2ca15c6 206 gps.attach(gps_rx,Serial::RxIrq);
m5171135 0:47edd2ca15c6 207 xbee.attach(xbee_rx,Serial::RxIrq);
m5171135 0:47edd2ca15c6 208 wait(5);
m5171135 0:47edd2ca15c6 209
m5171135 0:47edd2ca15c6 210 //Encorder Innterrapt Setting
m5171135 0:47edd2ca15c6 211 encoder1_A.rise(&flip);
m5171135 0:47edd2ca15c6 212
m5171135 0:47edd2ca15c6 213 //gyro_registor Setting
m5171135 0:47edd2ca15c6 214 char PWR_M[2]={0x3E,0x80};
m5171135 0:47edd2ca15c6 215 i2c.write(gyro_addr, PWR_M, 2); // Send command string
m5171135 0:47edd2ca15c6 216 char SMPL[2]={0x15,0x00};
m5171135 0:47edd2ca15c6 217 i2c.write(gyro_addr, SMPL, 2); // Send command string
m5171135 0:47edd2ca15c6 218 char DLPF[2]={0x16,0x18};
m5171135 0:47edd2ca15c6 219 i2c.write(gyro_addr, DLPF, 2); // Send command string
m5171135 0:47edd2ca15c6 220 char INT_C[2]={0x17,0x05};
m5171135 0:47edd2ca15c6 221 i2c.write(gyro_addr, INT_C, 2); // Send commanad string
m5171135 0:47edd2ca15c6 222 char PWR_M2[2]={0x3E,0x00};
m5171135 0:47edd2ca15c6 223 i2c.write(gyro_addr, PWR_M2, 2); // Send command string
m5171135 0:47edd2ca15c6 224
m5171135 0:47edd2ca15c6 225 wait(1);
m5171135 0:47edd2ca15c6 226 pc.printf("start\n");
m5171135 0:47edd2ca15c6 227 axis.attach(&axisRenovation, 0.1);
m5171135 0:47edd2ca15c6 228
m5171135 0:47edd2ca15c6 229 motor1_pwm = 0.0;
m5171135 0:47edd2ca15c6 230 //motor2_pwm = 0.3;
m5171135 0:47edd2ca15c6 231
m5171135 0:47edd2ca15c6 232
m5171135 0:47edd2ca15c6 233 while (1) {
m5171135 0:47edd2ca15c6 234
m5171135 0:47edd2ca15c6 235 }
m5171135 0:47edd2ca15c6 236 }