robot
Dependencies: ADXL345 AigamozuControlPackets HMC5843 ITG3200 MBed_Adafruit-GPS-Library XBee agzIDLIST mbed agz_common
main.cpp@0:7d55d6ace996, 2014-05-20 (annotated)
- Committer:
- m5171135
- Date:
- Tue May 20 14:51:25 2014 +0000
- Revision:
- 0:7d55d6ace996
- Child:
- 1:490b793b2e61
aaa
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
m5171135 | 0:7d55d6ace996 | 1 | #include "mbed.h" |
m5171135 | 0:7d55d6ace996 | 2 | #include "XBee.h" |
m5171135 | 0:7d55d6ace996 | 3 | #include "MBed_Adafruit_GPS.h" |
m5171135 | 0:7d55d6ace996 | 4 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 5 | // |
m5171135 | 0:7d55d6ace996 | 6 | //Connection Setting |
m5171135 | 0:7d55d6ace996 | 7 | // |
m5171135 | 0:7d55d6ace996 | 8 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 9 | DigitalOut led1(LED1); |
m5171135 | 0:7d55d6ace996 | 10 | //IMU -> i2c |
m5171135 | 0:7d55d6ace996 | 11 | I2C i2c(p9, p10); |
m5171135 | 0:7d55d6ace996 | 12 | //set serial |
m5171135 | 0:7d55d6ace996 | 13 | Serial pc(USBTX, USBRX); //tx, rx |
m5171135 | 0:7d55d6ace996 | 14 | XBee xbee(p13,p14); //tx,rx |
m5171135 | 0:7d55d6ace996 | 15 | Serial * gps_Serial; |
m5171135 | 0:7d55d6ace996 | 16 | //gps responce |
m5171135 | 0:7d55d6ace996 | 17 | ZBRxResponse zbRx = ZBRxResponse(); |
m5171135 | 0:7d55d6ace996 | 18 | XBeeAddress64 remoteAddress = XBeeAddress64(0x0013A200,0x40991C67); |
m5171135 | 0:7d55d6ace996 | 19 | |
m5171135 | 0:7d55d6ace996 | 20 | |
m5171135 | 0:7d55d6ace996 | 21 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 22 | // |
m5171135 | 0:7d55d6ace996 | 23 | //Pin Setting |
m5171135 | 0:7d55d6ace996 | 24 | // |
m5171135 | 0:7d55d6ace996 | 25 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 26 | |
m5171135 | 0:7d55d6ace996 | 27 | //Motor & Encorder Setting |
m5171135 | 0:7d55d6ace996 | 28 | //Left |
m5171135 | 0:7d55d6ace996 | 29 | DigitalOut motorL_A(p21); |
m5171135 | 0:7d55d6ace996 | 30 | DigitalOut motorL_B(p22); |
m5171135 | 0:7d55d6ace996 | 31 | PwmOut motorL_pwm(p23); |
m5171135 | 0:7d55d6ace996 | 32 | InterruptIn encoderR_A(p15); |
m5171135 | 0:7d55d6ace996 | 33 | InterruptIn encoderR_B(p16); |
m5171135 | 0:7d55d6ace996 | 34 | |
m5171135 | 0:7d55d6ace996 | 35 | //Rignt |
m5171135 | 0:7d55d6ace996 | 36 | DigitalOut motorR_A(p24); |
m5171135 | 0:7d55d6ace996 | 37 | DigitalOut motorR_B(p25); |
m5171135 | 0:7d55d6ace996 | 38 | PwmOut motorR_pwm(p26); |
m5171135 | 0:7d55d6ace996 | 39 | |
m5171135 | 0:7d55d6ace996 | 40 | //test data |
m5171135 | 0:7d55d6ace996 | 41 | Ticker axis; |
m5171135 | 0:7d55d6ace996 | 42 | Ticker auth_axis; |
m5171135 | 0:7d55d6ace996 | 43 | |
m5171135 | 0:7d55d6ace996 | 44 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 45 | // |
m5171135 | 0:7d55d6ace996 | 46 | //Each Value Setting |
m5171135 | 0:7d55d6ace996 | 47 | // |
m5171135 | 0:7d55d6ace996 | 48 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 49 | //my status |
m5171135 | 0:7d55d6ace996 | 50 | //0: StndbyMode |
m5171135 | 0:7d55d6ace996 | 51 | //1: ManualMode |
m5171135 | 0:7d55d6ace996 | 52 | //2: AuthmaticMode(Random) |
m5171135 | 0:7d55d6ace996 | 53 | unsigned char my_status = 0; |
m5171135 | 0:7d55d6ace996 | 54 | |
m5171135 | 0:7d55d6ace996 | 55 | //my_status |
m5171135 | 0:7d55d6ace996 | 56 | // 0 -> stable |
m5171135 | 0:7d55d6ace996 | 57 | // 1 -> error |
m5171135 | 0:7d55d6ace996 | 58 | |
m5171135 | 0:7d55d6ace996 | 59 | //0 bit: Motor Satatus |
m5171135 | 0:7d55d6ace996 | 60 | //1 bit: GPS Status |
m5171135 | 0:7d55d6ace996 | 61 | //2 bit: Sensor Status |
m5171135 | 0:7d55d6ace996 | 62 | //3 bit: Battery Status |
m5171135 | 0:7d55d6ace996 | 63 | //4 bit: |
m5171135 | 0:7d55d6ace996 | 64 | //5 bit: |
m5171135 | 0:7d55d6ace996 | 65 | //6 bit: |
m5171135 | 0:7d55d6ace996 | 66 | //7 bit: |
m5171135 | 0:7d55d6ace996 | 67 | |
m5171135 | 0:7d55d6ace996 | 68 | unsigned char my_mode = 0; |
m5171135 | 0:7d55d6ace996 | 69 | |
m5171135 | 0:7d55d6ace996 | 70 | //I2C address 9-axis |
m5171135 | 0:7d55d6ace996 | 71 | const int gyro_addr = 0xD0; |
m5171135 | 0:7d55d6ace996 | 72 | const int acc_addr = 0xA6; |
m5171135 | 0:7d55d6ace996 | 73 | |
m5171135 | 0:7d55d6ace996 | 74 | char gyro_head[1]; |
m5171135 | 0:7d55d6ace996 | 75 | char read[8]; |
m5171135 | 0:7d55d6ace996 | 76 | short int gyro_x=0; |
m5171135 | 0:7d55d6ace996 | 77 | short int gyro_y=0; |
m5171135 | 0:7d55d6ace996 | 78 | short int gyro_z=0; |
m5171135 | 0:7d55d6ace996 | 79 | short int tempr=0; |
m5171135 | 0:7d55d6ace996 | 80 | |
m5171135 | 0:7d55d6ace996 | 81 | char acc_head[1]; |
m5171135 | 0:7d55d6ace996 | 82 | char acc_buf[6]; |
m5171135 | 0:7d55d6ace996 | 83 | short int acc_x = 0; |
m5171135 | 0:7d55d6ace996 | 84 | short int acc_y = 0; |
m5171135 | 0:7d55d6ace996 | 85 | short int acc_z = 0; |
m5171135 | 0:7d55d6ace996 | 86 | |
m5171135 | 0:7d55d6ace996 | 87 | //GPS value |
m5171135 | 0:7d55d6ace996 | 88 | //float longitude = 0.0; |
m5171135 | 0:7d55d6ace996 | 89 | //float latitude = 0.0; |
m5171135 | 0:7d55d6ace996 | 90 | |
m5171135 | 0:7d55d6ace996 | 91 | //motor_speed_feed_back |
m5171135 | 0:7d55d6ace996 | 92 | float target_palse = 0.0; |
m5171135 | 0:7d55d6ace996 | 93 | float pwm; |
m5171135 | 0:7d55d6ace996 | 94 | long encorder_count = 0; |
m5171135 | 0:7d55d6ace996 | 95 | int count = 0; |
m5171135 | 0:7d55d6ace996 | 96 | |
m5171135 | 0:7d55d6ace996 | 97 | //ManualValue |
m5171135 | 0:7d55d6ace996 | 98 | int manual_count = 0; |
m5171135 | 0:7d55d6ace996 | 99 | int manual_flag = 0; |
m5171135 | 0:7d55d6ace996 | 100 | |
m5171135 | 0:7d55d6ace996 | 101 | |
m5171135 | 0:7d55d6ace996 | 102 | //test value |
m5171135 | 0:7d55d6ace996 | 103 | long sub_latH = 12345; |
m5171135 | 0:7d55d6ace996 | 104 | long sub_latL = 67890; |
m5171135 | 0:7d55d6ace996 | 105 | |
m5171135 | 0:7d55d6ace996 | 106 | long sub_lonH = 98765; |
m5171135 | 0:7d55d6ace996 | 107 | long sub_lonL = 43211; |
m5171135 | 0:7d55d6ace996 | 108 | |
m5171135 | 0:7d55d6ace996 | 109 | |
m5171135 | 0:7d55d6ace996 | 110 | union UNI_TEST_T |
m5171135 | 0:7d55d6ace996 | 111 | { |
m5171135 | 0:7d55d6ace996 | 112 | long a; |
m5171135 | 0:7d55d6ace996 | 113 | uint8_t b[4]; |
m5171135 | 0:7d55d6ace996 | 114 | }; |
m5171135 | 0:7d55d6ace996 | 115 | |
m5171135 | 0:7d55d6ace996 | 116 | |
m5171135 | 0:7d55d6ace996 | 117 | |
m5171135 | 0:7d55d6ace996 | 118 | |
m5171135 | 0:7d55d6ace996 | 119 | UNI_TEST_T latH,latL,lonH,lonL; |
m5171135 | 0:7d55d6ace996 | 120 | |
m5171135 | 0:7d55d6ace996 | 121 | |
m5171135 | 0:7d55d6ace996 | 122 | |
m5171135 | 0:7d55d6ace996 | 123 | |
m5171135 | 0:7d55d6ace996 | 124 | |
m5171135 | 0:7d55d6ace996 | 125 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 126 | // |
m5171135 | 0:7d55d6ace996 | 127 | //Change Motor State Processing |
m5171135 | 0:7d55d6ace996 | 128 | // |
m5171135 | 0:7d55d6ace996 | 129 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 130 | void change_speed(unsigned char L_state,unsigned char L_pwm,unsigned char R_state,unsigned char R_pwm){ |
m5171135 | 0:7d55d6ace996 | 131 | switch(L_state){ |
m5171135 | 0:7d55d6ace996 | 132 | case 0: |
m5171135 | 0:7d55d6ace996 | 133 | motorL_A = 0; |
m5171135 | 0:7d55d6ace996 | 134 | motorL_B = 0; |
m5171135 | 0:7d55d6ace996 | 135 | break; |
m5171135 | 0:7d55d6ace996 | 136 | |
m5171135 | 0:7d55d6ace996 | 137 | case 1: |
m5171135 | 0:7d55d6ace996 | 138 | motorL_A = 1; |
m5171135 | 0:7d55d6ace996 | 139 | motorL_B = 0; |
m5171135 | 0:7d55d6ace996 | 140 | break; |
m5171135 | 0:7d55d6ace996 | 141 | |
m5171135 | 0:7d55d6ace996 | 142 | case 2: |
m5171135 | 0:7d55d6ace996 | 143 | motorL_A = 0; |
m5171135 | 0:7d55d6ace996 | 144 | motorL_B = 1; |
m5171135 | 0:7d55d6ace996 | 145 | break; |
m5171135 | 0:7d55d6ace996 | 146 | |
m5171135 | 0:7d55d6ace996 | 147 | default: |
m5171135 | 0:7d55d6ace996 | 148 | motorL_A = 0; |
m5171135 | 0:7d55d6ace996 | 149 | motorL_B = 0; |
m5171135 | 0:7d55d6ace996 | 150 | break; |
m5171135 | 0:7d55d6ace996 | 151 | } |
m5171135 | 0:7d55d6ace996 | 152 | |
m5171135 | 0:7d55d6ace996 | 153 | motorL_pwm = (float)L_pwm/256.0; |
m5171135 | 0:7d55d6ace996 | 154 | |
m5171135 | 0:7d55d6ace996 | 155 | switch(R_state){ |
m5171135 | 0:7d55d6ace996 | 156 | case 0: |
m5171135 | 0:7d55d6ace996 | 157 | motorR_A = 0; |
m5171135 | 0:7d55d6ace996 | 158 | motorR_B = 0; |
m5171135 | 0:7d55d6ace996 | 159 | break; |
m5171135 | 0:7d55d6ace996 | 160 | |
m5171135 | 0:7d55d6ace996 | 161 | case 1: |
m5171135 | 0:7d55d6ace996 | 162 | motorR_A = 1; |
m5171135 | 0:7d55d6ace996 | 163 | motorR_B = 0; |
m5171135 | 0:7d55d6ace996 | 164 | break; |
m5171135 | 0:7d55d6ace996 | 165 | |
m5171135 | 0:7d55d6ace996 | 166 | case 2: |
m5171135 | 0:7d55d6ace996 | 167 | motorR_A = 0; |
m5171135 | 0:7d55d6ace996 | 168 | motorR_B = 1; |
m5171135 | 0:7d55d6ace996 | 169 | break; |
m5171135 | 0:7d55d6ace996 | 170 | |
m5171135 | 0:7d55d6ace996 | 171 | default: |
m5171135 | 0:7d55d6ace996 | 172 | motorR_A = 0; |
m5171135 | 0:7d55d6ace996 | 173 | motorR_B = 0; |
m5171135 | 0:7d55d6ace996 | 174 | break; |
m5171135 | 0:7d55d6ace996 | 175 | } |
m5171135 | 0:7d55d6ace996 | 176 | motorR_pwm = (float)R_pwm/256.0; |
m5171135 | 0:7d55d6ace996 | 177 | |
m5171135 | 0:7d55d6ace996 | 178 | } |
m5171135 | 0:7d55d6ace996 | 179 | |
m5171135 | 0:7d55d6ace996 | 180 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 181 | // |
m5171135 | 0:7d55d6ace996 | 182 | //Interruption processing 1: time -> 0.1s |
m5171135 | 0:7d55d6ace996 | 183 | // |
m5171135 | 0:7d55d6ace996 | 184 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 185 | void axisRenovation(){ |
m5171135 | 0:7d55d6ace996 | 186 | //gyro |
m5171135 | 0:7d55d6ace996 | 187 | gyro_head[0] = 0x1B; |
m5171135 | 0:7d55d6ace996 | 188 | i2c.write(gyro_addr, gyro_head, 1); |
m5171135 | 0:7d55d6ace996 | 189 | i2c.read(gyro_addr, read, 8); |
m5171135 | 0:7d55d6ace996 | 190 | |
m5171135 | 0:7d55d6ace996 | 191 | tempr=(read[0] << 8) + read[1]; |
m5171135 | 0:7d55d6ace996 | 192 | gyro_x=(read[2] << 8) + read[3]; |
m5171135 | 0:7d55d6ace996 | 193 | gyro_y=(read[4] << 8) + read[5]; |
m5171135 | 0:7d55d6ace996 | 194 | gyro_z=(read[6] << 8) + read[7]; |
m5171135 | 0:7d55d6ace996 | 195 | |
m5171135 | 0:7d55d6ace996 | 196 | //acc |
m5171135 | 0:7d55d6ace996 | 197 | acc_head[0] = 0x32; |
m5171135 | 0:7d55d6ace996 | 198 | i2c.write(acc_addr,acc_head,1); |
m5171135 | 0:7d55d6ace996 | 199 | i2c.read(acc_addr, acc_buf, 6); |
m5171135 | 0:7d55d6ace996 | 200 | |
m5171135 | 0:7d55d6ace996 | 201 | acc_x = (acc_buf[1] << 8) + acc_buf[0]; |
m5171135 | 0:7d55d6ace996 | 202 | acc_y = (acc_buf[3] << 8) + acc_buf[2]; |
m5171135 | 0:7d55d6ace996 | 203 | acc_z = (acc_buf[5] << 8) + acc_buf[4]; |
m5171135 | 0:7d55d6ace996 | 204 | |
m5171135 | 0:7d55d6ace996 | 205 | |
m5171135 | 0:7d55d6ace996 | 206 | if(manual_count > 100){ |
m5171135 | 0:7d55d6ace996 | 207 | change_speed(0,0,0,0); |
m5171135 | 0:7d55d6ace996 | 208 | manual_count = 0; |
m5171135 | 0:7d55d6ace996 | 209 | } |
m5171135 | 0:7d55d6ace996 | 210 | |
m5171135 | 0:7d55d6ace996 | 211 | manual_count++; |
m5171135 | 0:7d55d6ace996 | 212 | |
m5171135 | 0:7d55d6ace996 | 213 | //printf("Mode -> %d\n", my_mode); |
m5171135 | 0:7d55d6ace996 | 214 | //printf("Status -> %d\n", my_status); |
m5171135 | 0:7d55d6ace996 | 215 | //printf("Status -> %d\n", count); |
m5171135 | 0:7d55d6ace996 | 216 | |
m5171135 | 0:7d55d6ace996 | 217 | } |
m5171135 | 0:7d55d6ace996 | 218 | |
m5171135 | 0:7d55d6ace996 | 219 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 220 | // |
m5171135 | 0:7d55d6ace996 | 221 | //Interruption processing: time -> 1.0s |
m5171135 | 0:7d55d6ace996 | 222 | // |
m5171135 | 0:7d55d6ace996 | 223 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 224 | |
m5171135 | 0:7d55d6ace996 | 225 | void randomRenovation(){ |
m5171135 | 0:7d55d6ace996 | 226 | |
m5171135 | 0:7d55d6ace996 | 227 | if(count < 30){ |
m5171135 | 0:7d55d6ace996 | 228 | change_speed(1,127,1,127); |
m5171135 | 0:7d55d6ace996 | 229 | } |
m5171135 | 0:7d55d6ace996 | 230 | |
m5171135 | 0:7d55d6ace996 | 231 | else{ |
m5171135 | 0:7d55d6ace996 | 232 | change_speed(1,127,2,127); |
m5171135 | 0:7d55d6ace996 | 233 | if(count > 35) count = 0; |
m5171135 | 0:7d55d6ace996 | 234 | } |
m5171135 | 0:7d55d6ace996 | 235 | count++; |
m5171135 | 0:7d55d6ace996 | 236 | } |
m5171135 | 0:7d55d6ace996 | 237 | |
m5171135 | 0:7d55d6ace996 | 238 | |
m5171135 | 0:7d55d6ace996 | 239 | |
m5171135 | 0:7d55d6ace996 | 240 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 241 | // |
m5171135 | 0:7d55d6ace996 | 242 | //Main Processing |
m5171135 | 0:7d55d6ace996 | 243 | // |
m5171135 | 0:7d55d6ace996 | 244 | ///////////////////////////////////////// |
m5171135 | 0:7d55d6ace996 | 245 | int main() { |
m5171135 | 0:7d55d6ace996 | 246 | //start up time |
m5171135 | 0:7d55d6ace996 | 247 | wait(3); |
m5171135 | 0:7d55d6ace996 | 248 | //set i2c frequency to 400 KHz |
m5171135 | 0:7d55d6ace996 | 249 | i2c.frequency(400000); |
m5171135 | 0:7d55d6ace996 | 250 | //set pc frequency to 57600bps |
m5171135 | 0:7d55d6ace996 | 251 | pc.baud(57600); |
m5171135 | 0:7d55d6ace996 | 252 | //set xbee frequency to 57600bps |
m5171135 | 0:7d55d6ace996 | 253 | xbee.begin(57600); |
m5171135 | 0:7d55d6ace996 | 254 | |
m5171135 | 0:7d55d6ace996 | 255 | //GPS setting |
m5171135 | 0:7d55d6ace996 | 256 | gps_Serial = new Serial(p28,p27); //serial object for use w/ GPS |
m5171135 | 0:7d55d6ace996 | 257 | Adafruit_GPS myGPS(gps_Serial); //object of Adafruit's GPS class |
m5171135 | 0:7d55d6ace996 | 258 | //char c; //when read via Adafruit_GPS::read(), the class returns single character stored here |
m5171135 | 0:7d55d6ace996 | 259 | Timer refresh_Timer; //sets up a timer for use in loop; how often do we print GPS info? |
m5171135 | 0:7d55d6ace996 | 260 | const int refresh_Time = 2000; //refresh time in ms |
m5171135 | 0:7d55d6ace996 | 261 | |
m5171135 | 0:7d55d6ace996 | 262 | myGPS.begin(9600); //sets baud rate for GPS communication; note this may be changed via Adafruit_GPS::sendCommand(char *) |
m5171135 | 0:7d55d6ace996 | 263 | //a list of GPS commands is available at http://www.adafruit.com/datasheets/PMTK_A08.pdf |
m5171135 | 0:7d55d6ace996 | 264 | |
m5171135 | 0:7d55d6ace996 | 265 | myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY); //these commands are defined in MBed_Adafruit_GPS.h; a link is provided there for command creation |
m5171135 | 0:7d55d6ace996 | 266 | myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); |
m5171135 | 0:7d55d6ace996 | 267 | myGPS.sendCommand(PGCMD_ANTENNA); |
m5171135 | 0:7d55d6ace996 | 268 | |
m5171135 | 0:7d55d6ace996 | 269 | //gyro_registor Setting |
m5171135 | 0:7d55d6ace996 | 270 | char PWR_M[2]={0x3E,0x80}; |
m5171135 | 0:7d55d6ace996 | 271 | i2c.write(gyro_addr, PWR_M, 2); // Send command string |
m5171135 | 0:7d55d6ace996 | 272 | char SMPL[2]={0x15,0x00}; |
m5171135 | 0:7d55d6ace996 | 273 | i2c.write(gyro_addr, SMPL, 2); // Send command string |
m5171135 | 0:7d55d6ace996 | 274 | char DLPF[2]={0x16,0x18}; |
m5171135 | 0:7d55d6ace996 | 275 | i2c.write(gyro_addr, DLPF, 2); // Send command string |
m5171135 | 0:7d55d6ace996 | 276 | char INT_C[2]={0x17,0x05}; |
m5171135 | 0:7d55d6ace996 | 277 | i2c.write(gyro_addr, INT_C, 2); // Send commanad string |
m5171135 | 0:7d55d6ace996 | 278 | char PWR_M2[2]={0x3E,0x00}; |
m5171135 | 0:7d55d6ace996 | 279 | i2c.write(gyro_addr, PWR_M2, 2); // Send command string |
m5171135 | 0:7d55d6ace996 | 280 | |
m5171135 | 0:7d55d6ace996 | 281 | wait(2); |
m5171135 | 0:7d55d6ace996 | 282 | |
m5171135 | 0:7d55d6ace996 | 283 | //interrupt start |
m5171135 | 0:7d55d6ace996 | 284 | axis.attach(&axisRenovation, 0.1); |
m5171135 | 0:7d55d6ace996 | 285 | motorL_pwm = 0.0; |
m5171135 | 0:7d55d6ace996 | 286 | motorR_pwm = 0.0; |
m5171135 | 0:7d55d6ace996 | 287 | |
m5171135 | 0:7d55d6ace996 | 288 | refresh_Timer.start(); |
m5171135 | 0:7d55d6ace996 | 289 | |
m5171135 | 0:7d55d6ace996 | 290 | while (1) { |
m5171135 | 0:7d55d6ace996 | 291 | |
m5171135 | 0:7d55d6ace996 | 292 | //recive xbee module |
m5171135 | 0:7d55d6ace996 | 293 | xbee.readPacket(); |
m5171135 | 0:7d55d6ace996 | 294 | if (xbee.getResponse().isAvailable()) { |
m5171135 | 0:7d55d6ace996 | 295 | if (xbee.getResponse().getApiId() == ZB_RX_RESPONSE) { |
m5171135 | 0:7d55d6ace996 | 296 | xbee.getResponse().getZBRxResponse(zbRx); |
m5171135 | 0:7d55d6ace996 | 297 | unsigned char *buf = zbRx.getFrameData(); |
m5171135 | 0:7d55d6ace996 | 298 | |
m5171135 | 0:7d55d6ace996 | 299 | switch((unsigned char)buf[14]){ |
m5171135 | 0:7d55d6ace996 | 300 | |
m5171135 | 0:7d55d6ace996 | 301 | //ChanegeMode |
m5171135 | 0:7d55d6ace996 | 302 | case 'C':{ |
m5171135 | 0:7d55d6ace996 | 303 | my_mode = buf[19]; |
m5171135 | 0:7d55d6ace996 | 304 | change_speed(0,0,0,0); |
m5171135 | 0:7d55d6ace996 | 305 | |
m5171135 | 0:7d55d6ace996 | 306 | if(my_mode == 2) auth_axis.attach(&randomRenovation, 1.0); |
m5171135 | 0:7d55d6ace996 | 307 | else auth_axis.detach(); |
m5171135 | 0:7d55d6ace996 | 308 | break; |
m5171135 | 0:7d55d6ace996 | 309 | } |
m5171135 | 0:7d55d6ace996 | 310 | |
m5171135 | 0:7d55d6ace996 | 311 | //MunualCommand |
m5171135 | 0:7d55d6ace996 | 312 | case 'M':{ |
m5171135 | 0:7d55d6ace996 | 313 | if(my_mode == 1){ |
m5171135 | 0:7d55d6ace996 | 314 | manual_count = 0; |
m5171135 | 0:7d55d6ace996 | 315 | change_speed(buf[20],buf[21],buf[23],buf[24]); |
m5171135 | 0:7d55d6ace996 | 316 | |
m5171135 | 0:7d55d6ace996 | 317 | } |
m5171135 | 0:7d55d6ace996 | 318 | break; |
m5171135 | 0:7d55d6ace996 | 319 | } |
m5171135 | 0:7d55d6ace996 | 320 | |
m5171135 | 0:7d55d6ace996 | 321 | //SendStatus |
m5171135 | 0:7d55d6ace996 | 322 | case 'S':{ |
m5171135 | 0:7d55d6ace996 | 323 | //latH.a = myGPS.latitudeH;; |
m5171135 | 0:7d55d6ace996 | 324 | //latL.a = myGPS.latitudeL; |
m5171135 | 0:7d55d6ace996 | 325 | //lonH.a = myGPS.longitudeH; |
m5171135 | 0:7d55d6ace996 | 326 | //lonL.a = myGPS.longitudeL; |
m5171135 | 0:7d55d6ace996 | 327 | led1 = 1; |
m5171135 | 0:7d55d6ace996 | 328 | //dummy data |
m5171135 | 0:7d55d6ace996 | 329 | latH.a = sub_latH; |
m5171135 | 0:7d55d6ace996 | 330 | latL.a = sub_latL; |
m5171135 | 0:7d55d6ace996 | 331 | lonH.a = sub_lonH; |
m5171135 | 0:7d55d6ace996 | 332 | lonL.a = sub_lonL; |
m5171135 | 0:7d55d6ace996 | 333 | |
m5171135 | 0:7d55d6ace996 | 334 | uint8_t data[] = {65,71,83,82,70,65,84,66,83,my_status,71,80,83,latH.b[0],latH.b[1],latH.b[2],latH.b[3],latL.b[0],latL.b[1],latL.b[2],latL.b[3],lonH.b[0],lonH.b[1],lonH.b[2],lonH.b[3],lonL.b[0],lonL.b[1],lonL.b[2],lonL.b[3],65,71,69}; |
m5171135 | 0:7d55d6ace996 | 335 | ZBTxRequest tx64request(remoteAddress,data,sizeof(data)); |
m5171135 | 0:7d55d6ace996 | 336 | xbee.send(tx64request); |
m5171135 | 0:7d55d6ace996 | 337 | break; |
m5171135 | 0:7d55d6ace996 | 338 | } |
m5171135 | 0:7d55d6ace996 | 339 | |
m5171135 | 0:7d55d6ace996 | 340 | default: |
m5171135 | 0:7d55d6ace996 | 341 | { |
m5171135 | 0:7d55d6ace996 | 342 | break; |
m5171135 | 0:7d55d6ace996 | 343 | } |
m5171135 | 0:7d55d6ace996 | 344 | } |
m5171135 | 0:7d55d6ace996 | 345 | } |
m5171135 | 0:7d55d6ace996 | 346 | } |
m5171135 | 0:7d55d6ace996 | 347 | |
m5171135 | 0:7d55d6ace996 | 348 | myGPS.read(); |
m5171135 | 0:7d55d6ace996 | 349 | //recive gps module |
m5171135 | 0:7d55d6ace996 | 350 | //check if we recieved a new message from GPS, if so, attempt to parse it, |
m5171135 | 0:7d55d6ace996 | 351 | if ( myGPS.newNMEAreceived() ) { |
m5171135 | 0:7d55d6ace996 | 352 | if ( !myGPS.parse(myGPS.lastNMEA()) ) { |
m5171135 | 0:7d55d6ace996 | 353 | continue; |
m5171135 | 0:7d55d6ace996 | 354 | } |
m5171135 | 0:7d55d6ace996 | 355 | } |
m5171135 | 0:7d55d6ace996 | 356 | |
m5171135 | 0:7d55d6ace996 | 357 | if (refresh_Timer.read_ms() >= refresh_Time) { |
m5171135 | 0:7d55d6ace996 | 358 | refresh_Timer.reset(); |
m5171135 | 0:7d55d6ace996 | 359 | if (myGPS.fix) { |
m5171135 | 0:7d55d6ace996 | 360 | my_status = 0; |
m5171135 | 0:7d55d6ace996 | 361 | //longitude = myGPS.longitudeH; |
m5171135 | 0:7d55d6ace996 | 362 | //latitude = myGPS.latitudeH; |
m5171135 | 0:7d55d6ace996 | 363 | } |
m5171135 | 0:7d55d6ace996 | 364 | |
m5171135 | 0:7d55d6ace996 | 365 | else my_status = 1; |
m5171135 | 0:7d55d6ace996 | 366 | } |
m5171135 | 0:7d55d6ace996 | 367 | } |
m5171135 | 0:7d55d6ace996 | 368 | } |