Seokyong Song / Mbed 2 deprecated videoray_command

d

Dependencies:   Command mbed-rtos mbed

main.cpp@0:405f170eac1c, 2016-05-09 (annotated)

Committer:
gosari
Date:
Mon May 09 06:08:14 2016 +0000
Revision:
0:405f170eac1c
d

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gosari 0:405f170eac1c 1 #include "mbed.h"
gosari 0:405f170eac1c 2 #include "CAN.h"
gosari 0:405f170eac1c 3 #include "Command.h"
gosari 0:405f170eac1c 4 #include "MS3DMGX2.h"
gosari 0:405f170eac1c 5 #include "rtos.h"
gosari 0:405f170eac1c 6
gosari 0:405f170eac1c 7 #define PI 3.141592
gosari 0:405f170eac1c 8
gosari 0:405f170eac1c 9 typedef unsigned char BYTE;
gosari 0:405f170eac1c 10
gosari 0:405f170eac1c 11 Serial pc(USBTX, USBRX);
gosari 0:405f170eac1c 12 CAN can(p30, p29);
gosari 0:405f170eac1c 13 DigitalOut led1(LED1);
gosari 0:405f170eac1c 14 Serial imu(p13, p14);
gosari 0:405f170eac1c 15
gosari 0:405f170eac1c 16 int handle1;
gosari 0:405f170eac1c 17
gosari 0:405f170eac1c 18 BYTE* Temp;
gosari 0:405f170eac1c 19
gosari 0:405f170eac1c 20 int Read_Data[3] = {0,0,0};
gosari 0:405f170eac1c 21 int DEPTH = 0;
gosari 0:405f170eac1c 22 int HEADING = 0;
gosari 0:405f170eac1c 23
gosari 0:405f170eac1c 24 int flag_break;
gosari 0:405f170eac1c 25
gosari 0:405f170eac1c 26 float data[6];
gosari 0:405f170eac1c 27
gosari 0:405f170eac1c 28 unsigned char Buffer[31];
gosari 0:405f170eac1c 29 unsigned char PacketSize;
gosari 0:405f170eac1c 30 bool Checksum(unsigned char Index, unsigned char Length);
gosari 0:405f170eac1c 31
gosari 0:405f170eac1c 32 unsigned char BufferStart;
gosari 0:405f170eac1c 33 unsigned char BufferEnd;
gosari 0:405f170eac1c 34 unsigned char CommandByte;
gosari 0:405f170eac1c 35 unsigned char ResponseLength;
gosari 0:405f170eac1c 36
gosari 0:405f170eac1c 37
gosari 0:405f170eac1c 38
gosari 0:405f170eac1c 39 void imu_thread(void const *args){
gosari 0:405f170eac1c 40 while(true){
gosari 0:405f170eac1c 41 led1=!led1;
gosari 0:405f170eac1c 42
gosari 0:405f170eac1c 43 //while(!IMU.Readable());
gosari 0:405f170eac1c 44 //isvalid = IMU.Read(data);
gosari 0:405f170eac1c 45
gosari 0:405f170eac1c 46 BufferStart = 1;
gosari 0:405f170eac1c 47 BufferEnd = 0;
gosari 0:405f170eac1c 48
gosari 0:405f170eac1c 49 // Write Command
gosari 0:405f170eac1c 50 imu.putc(CommandByte);
gosari 0:405f170eac1c 51
gosari 0:405f170eac1c 52 // Read
gosari 0:405f170eac1c 53 do
gosari 0:405f170eac1c 54 {
gosari 0:405f170eac1c 55 Buffer[BufferEnd] = imu.getc();
gosari 0:405f170eac1c 56
gosari 0:405f170eac1c 57 //pc.putc(Buffer[BufferEnd]);
gosari 0:405f170eac1c 58 BufferEnd++;
gosari 0:405f170eac1c 59
gosari 0:405f170eac1c 60 BufferEnd %= PacketSize;
gosari 0:405f170eac1c 61 } while(imu.readable());
gosari 0:405f170eac1c 62
gosari 0:405f170eac1c 63 // Calculate Checksum
gosari 0:405f170eac1c 64 //unsigned short Sum = 0;
gosari 0:405f170eac1c 65 //for (unsigned char a = 0; a < PacketSize - 2; a++)
gosari 0:405f170eac1c 66 //{
gosari 0:405f170eac1c 67 // Sum += Buffer[BufferStart];
gosari 0:405f170eac1c 68 // BufferStart++;
gosari 0:405f170eac1c 69 // BufferStart %= PacketSize;
gosari 0:405f170eac1c 70 //}
gosari 0:405f170eac1c 71 //bool checksum = (((unsigned char*)&Sum)[0] == Buffer[BufferStart+1]) && (((unsigned char*)&Sum)[1] == Buffer[BufferStart]);
gosari 0:405f170eac1c 72
gosari 0:405f170eac1c 73 //BufferStart = 1;
gosari 0:405f170eac1c 74
gosari 0:405f170eac1c 75 for (unsigned int i = 0; i < ResponseLength; i++)
gosari 0:405f170eac1c 76 {
gosari 0:405f170eac1c 77 for(unsigned int j = 3; j < 4; j--)
gosari 0:405f170eac1c 78 {
gosari 0:405f170eac1c 79 ((unsigned char*)&data[i])[j] = Buffer[BufferStart];
gosari 0:405f170eac1c 80 BufferStart++;
gosari 0:405f170eac1c 81 BufferStart %= PacketSize;
gosari 0:405f170eac1c 82 }
gosari 0:405f170eac1c 83 }
gosari 0:405f170eac1c 84 BufferStart += 6;
gosari 0:405f170eac1c 85 BufferStart %= PacketSize;
gosari 0:405f170eac1c 86
gosari 0:405f170eac1c 87
gosari 0:405f170eac1c 88 HEADING = (int)(data[2]*180.0/PI); // Heading = Yaw
gosari 0:405f170eac1c 89
gosari 0:405f170eac1c 90 Thread::wait(500);
gosari 0:405f170eac1c 91 }
gosari 0:405f170eac1c 92 }
gosari 0:405f170eac1c 93
gosari 0:405f170eac1c 94 int main(){
gosari 0:405f170eac1c 95
gosari 0:405f170eac1c 96 Command c;
gosari 0:405f170eac1c 97
gosari 0:405f170eac1c 98 pc.baud(9600);
gosari 0:405f170eac1c 99 can.frequency(148148);
gosari 0:405f170eac1c 100 imu.baud(115200);
gosari 0:405f170eac1c 101
gosari 0:405f170eac1c 102 CommandByte = 0xCF;
gosari 0:405f170eac1c 103 ResponseLength = 6;
gosari 0:405f170eac1c 104 PacketSize = 31;
gosari 0:405f170eac1c 105
gosari 0:405f170eac1c 106 pc.printf("Connected to mbed..\r\n");
gosari 0:405f170eac1c 107
gosari 0:405f170eac1c 108 //Thread thread(imu_thread);
gosari 0:405f170eac1c 109 //thread.set_priority(osPriorityHigh);
gosari 0:405f170eac1c 110 pc.printf("IMU thread start..\r\n");
gosari 0:405f170eac1c 111
gosari 0:405f170eac1c 112
gosari 0:405f170eac1c 113
gosari 0:405f170eac1c 114 while(1)
gosari 0:405f170eac1c 115 {
gosari 0:405f170eac1c 116 pc.printf("-----------------------\r\n");
gosari 0:405f170eac1c 117 pc.printf("All Stop = 0\r\n");
gosari 0:405f170eac1c 118 pc.printf("Light On = 1\r\n");
gosari 0:405f170eac1c 119 pc.printf("Heave = 2\r\n");
gosari 0:405f170eac1c 120 pc.printf("Surge = 3\r\n");
gosari 0:405f170eac1c 121 pc.printf("Heading PID_TEST = 4\r\n");
gosari 0:405f170eac1c 122 pc.printf("Depth PID_TEST = 5\r\n");
gosari 0:405f170eac1c 123 pc.printf("IMU Data Receive = 8\r\n");
gosari 0:405f170eac1c 124 pc.printf("-----------------------\r\n");
gosari 0:405f170eac1c 125
gosari 0:405f170eac1c 126
gosari 0:405f170eac1c 127 char stop = NULL;
gosari 0:405f170eac1c 128 stop = pc.getc();
gosari 0:405f170eac1c 129
gosari 0:405f170eac1c 130 if(stop == '0')
gosari 0:405f170eac1c 131 {
gosari 0:405f170eac1c 132 // All Stop
gosari 0:405f170eac1c 133 c.set();
gosari 0:405f170eac1c 134 pc.printf("%s\r\n", (char*)c.Re());
gosari 0:405f170eac1c 135 if(can.write(CANMessage(16, (char*)c.Re(), 6)))
gosari 0:405f170eac1c 136 {
gosari 0:405f170eac1c 137 pc.printf("All Stop command sent!\r\n");
gosari 0:405f170eac1c 138 }
gosari 0:405f170eac1c 139 stop = NULL;
gosari 0:405f170eac1c 140 }
gosari 0:405f170eac1c 141
gosari 0:405f170eac1c 142 else if(stop == '1')
gosari 0:405f170eac1c 143 {
gosari 0:405f170eac1c 144 // Light On
gosari 0:405f170eac1c 145 c.LIGHT(80);
gosari 0:405f170eac1c 146 pc.printf("%s\r\n", (char*)c.Re());
gosari 0:405f170eac1c 147 if(can.write(CANMessage(16, (char*)c.Re(), 6)))
gosari 0:405f170eac1c 148 {
gosari 0:405f170eac1c 149 pc.printf("Light On command sent!\r\n");
gosari 0:405f170eac1c 150 }
gosari 0:405f170eac1c 151 stop = NULL;
gosari 0:405f170eac1c 152 }
gosari 0:405f170eac1c 153
gosari 0:405f170eac1c 154 else if(stop == '2')
gosari 0:405f170eac1c 155 {
gosari 0:405f170eac1c 156 // Heave
gosari 0:405f170eac1c 157 int HV;
gosari 0:405f170eac1c 158 pc.printf("input Heave value = ");
gosari 0:405f170eac1c 159 pc.scanf("%d", &HV);
gosari 0:405f170eac1c 160 c.TH_H(HV * -1);
gosari 0:405f170eac1c 161 pc.printf("%s\r\n", (char*)c.Re());
gosari 0:405f170eac1c 162 if(can.write(CANMessage(16, (char*)c.Re(), 6)))
gosari 0:405f170eac1c 163 {
gosari 0:405f170eac1c 164 pc.printf("Heave %d command sent!\r\n", HV);
gosari 0:405f170eac1c 165 }
gosari 0:405f170eac1c 166 stop = NULL;
gosari 0:405f170eac1c 167 }
gosari 0:405f170eac1c 168
gosari 0:405f170eac1c 169 else if(stop == '3')
gosari 0:405f170eac1c 170 {
gosari 0:405f170eac1c 171 // Surge
gosari 0:405f170eac1c 172 c.TH_L(10);
gosari 0:405f170eac1c 173 c.TH_R(10);
gosari 0:405f170eac1c 174 pc.printf("%s\r\n", (char*)c.Re());
gosari 0:405f170eac1c 175 if(can.write(CANMessage(16, (char*)c.Re(), 6)))
gosari 0:405f170eac1c 176 {
gosari 0:405f170eac1c 177 pc.printf("Surge command sent!\r\n");
gosari 0:405f170eac1c 178 }
gosari 0:405f170eac1c 179 stop = NULL;
gosari 0:405f170eac1c 180 }
gosari 0:405f170eac1c 181
gosari 0:405f170eac1c 182 else if(stop == '4')
gosari 0:405f170eac1c 183 {
gosari 0:405f170eac1c 184 // PID_TEST
gosari 0:405f170eac1c 185 int T_HEADING = 0; // target heading value
gosari 0:405f170eac1c 186 pc.printf("input target heading (reference : %d) = ", HEADING);
gosari 0:405f170eac1c 187 pc.scanf("%d", &T_HEADING);
gosari 0:405f170eac1c 188 pc.printf("Press '9' to finish PID loop...\r\n");
gosari 0:405f170eac1c 189
gosari 0:405f170eac1c 190 int u = 0; // thruster input
gosari 0:405f170eac1c 191 float kp = 0.25; // gain
gosari 0:405f170eac1c 192 float ki = 0; // 0.005;
gosari 0:405f170eac1c 193 float kd = 0; // 0.25;
gosari 0:405f170eac1c 194
gosari 0:405f170eac1c 195 int Max_TH = 30; // maximum TH output
gosari 0:405f170eac1c 196 int integral = 0;
gosari 0:405f170eac1c 197
gosari 0:405f170eac1c 198 float E = 0;
gosari 0:405f170eac1c 199 float oldE = 0;
gosari 0:405f170eac1c 200 float oldEE = 0;
gosari 0:405f170eac1c 201
gosari 0:405f170eac1c 202 int h_flag = 1;
gosari 0:405f170eac1c 203 while(1)
gosari 0:405f170eac1c 204 {
gosari 0:405f170eac1c 205 char a = NULL;
gosari 0:405f170eac1c 206 if(pc.readable())
gosari 0:405f170eac1c 207 {
gosari 0:405f170eac1c 208 a = pc.getc();
gosari 0:405f170eac1c 209 if(a == '9'){
gosari 0:405f170eac1c 210 c.set();
gosari 0:405f170eac1c 211 if(can.write(CANMessage(16, (char*)c.Re(), 6)))
gosari 0:405f170eac1c 212 {
gosari 0:405f170eac1c 213 pc.printf("PID Loop Stopped!\r\n");
gosari 0:405f170eac1c 214 }
gosari 0:405f170eac1c 215 h_flag = 0;
gosari 0:405f170eac1c 216 break;
gosari 0:405f170eac1c 217
gosari 0:405f170eac1c 218 }
gosari 0:405f170eac1c 219 }
gosari 0:405f170eac1c 220
gosari 0:405f170eac1c 221 if(h_flag)
gosari 0:405f170eac1c 222 {
gosari 0:405f170eac1c 223 E = float(T_HEADING - HEADING);
gosari 0:405f170eac1c 224
gosari 0:405f170eac1c 225 if(E < -180){
gosari 0:405f170eac1c 226 E = E + 360;
gosari 0:405f170eac1c 227 }
gosari 0:405f170eac1c 228
gosari 0:405f170eac1c 229 u = integral + int(kp*(E - oldE) + ki*E + kd*((E - oldE) - (oldE - oldEE)));
gosari 0:405f170eac1c 230
gosari 0:405f170eac1c 231 oldEE = oldE;
gosari 0:405f170eac1c 232 oldE = E;
gosari 0:405f170eac1c 233 integral = u;
gosari 0:405f170eac1c 234
gosari 0:405f170eac1c 235 if(u > 0){
gosari 0:405f170eac1c 236 u = u + 6;
gosari 0:405f170eac1c 237 }
gosari 0:405f170eac1c 238 else if (u < 0){
gosari 0:405f170eac1c 239 u = u - 6;
gosari 0:405f170eac1c 240 }
gosari 0:405f170eac1c 241
gosari 0:405f170eac1c 242 if(u > Max_TH){
gosari 0:405f170eac1c 243 u = Max_TH;
gosari 0:405f170eac1c 244 }
gosari 0:405f170eac1c 245 else if (u < Max_TH*-1){
gosari 0:405f170eac1c 246 u = Max_TH * -1;
gosari 0:405f170eac1c 247 }
gosari 0:405f170eac1c 248 else if (u < 8 && u > -8){
gosari 0:405f170eac1c 249 u = 0;
gosari 0:405f170eac1c 250 }
gosari 0:405f170eac1c 251
gosari 0:405f170eac1c 252 c.TH_L(u*-1);
gosari 0:405f170eac1c 253 c.TH_R(u);
gosari 0:405f170eac1c 254
gosari 0:405f170eac1c 255 if(can.write(CANMessage(16, (char*)c.Re(), 6)))
gosari 0:405f170eac1c 256 {
gosari 0:405f170eac1c 257 pc.printf("Heading PID test command sent!\r\n");
gosari 0:405f170eac1c 258 pc.printf("output: %d\r\n", u);
gosari 0:405f170eac1c 259 pc.printf("error: %d\r\n", T_HEADING - HEADING);
gosari 0:405f170eac1c 260 }
gosari 0:405f170eac1c 261
gosari 0:405f170eac1c 262
gosari 0:405f170eac1c 263 } // if h_flag
gosari 0:405f170eac1c 264 } // PID loop
gosari 0:405f170eac1c 265
gosari 0:405f170eac1c 266 stop = NULL;
gosari 0:405f170eac1c 267
gosari 0:405f170eac1c 268 } // stop 4
gosari 0:405f170eac1c 269
gosari 0:405f170eac1c 270 else if(stop == '5')
gosari 0:405f170eac1c 271 {
gosari 0:405f170eac1c 272 // PID_TEST
gosari 0:405f170eac1c 273 int T_DEPTH = 0; // target depth value
gosari 0:405f170eac1c 274 pc.printf("input target depth (reference : %d) = ", DEPTH);
gosari 0:405f170eac1c 275 pc.scanf("%d", &T_DEPTH);
gosari 0:405f170eac1c 276 pc.printf("Press '9' to finish PID loop...\r\n");
gosari 0:405f170eac1c 277
gosari 0:405f170eac1c 278 int u = 0; // thruster input
gosari 0:405f170eac1c 279 float kp = 1; // 1.0
gosari 0:405f170eac1c 280 float ki = 0; // 0.005;
gosari 0:405f170eac1c 281 float kd = 0.1; // 0.25;
gosari 0:405f170eac1c 282
gosari 0:405f170eac1c 283 int Max_TH = 60; // maximum TH output
gosari 0:405f170eac1c 284 int integral = 0;
gosari 0:405f170eac1c 285
gosari 0:405f170eac1c 286 float E = 0;
gosari 0:405f170eac1c 287 float oldE = 0;
gosari 0:405f170eac1c 288 float oldEE = 0;
gosari 0:405f170eac1c 289
gosari 0:405f170eac1c 290 int firstRUN = 0;
gosari 0:405f170eac1c 291 float D_alpha = 0.7;
gosari 0:405f170eac1c 292 int prev_u = 0;
gosari 0:405f170eac1c 293 int curr_u = 0;
gosari 0:405f170eac1c 294
gosari 0:405f170eac1c 295 int d_flag = 1;
gosari 0:405f170eac1c 296 while(1)
gosari 0:405f170eac1c 297 {
gosari 0:405f170eac1c 298 if(pc.readable())
gosari 0:405f170eac1c 299 {
gosari 0:405f170eac1c 300 char a = NULL;
gosari 0:405f170eac1c 301 a = pc.getc();
gosari 0:405f170eac1c 302 if(a == '9')
gosari 0:405f170eac1c 303 {
gosari 0:405f170eac1c 304 c.set();
gosari 0:405f170eac1c 305 if(can.write(CANMessage(16, (char*)c.Re(), 6)))
gosari 0:405f170eac1c 306 {
gosari 0:405f170eac1c 307 pc.printf("PID Loop Stopped!\r\n");
gosari 0:405f170eac1c 308 }
gosari 0:405f170eac1c 309 d_flag = 0;
gosari 0:405f170eac1c 310 break;
gosari 0:405f170eac1c 311 }
gosari 0:405f170eac1c 312 }
gosari 0:405f170eac1c 313
gosari 0:405f170eac1c 314 if(d_flag)
gosari 0:405f170eac1c 315 {
gosari 0:405f170eac1c 316 E = float(T_DEPTH - DEPTH);
gosari 0:405f170eac1c 317
gosari 0:405f170eac1c 318 u = integral + int(kp*(E - oldE) + ki*E + kd*((E - oldE) - (oldE - oldEE)));
gosari 0:405f170eac1c 319
gosari 0:405f170eac1c 320 oldEE = oldE;
gosari 0:405f170eac1c 321 oldE = E;
gosari 0:405f170eac1c 322 integral = u;
gosari 0:405f170eac1c 323
gosari 0:405f170eac1c 324 if(u > Max_TH){
gosari 0:405f170eac1c 325 u = Max_TH;
gosari 0:405f170eac1c 326 }
gosari 0:405f170eac1c 327 else if (u < Max_TH*-1){
gosari 0:405f170eac1c 328 u = Max_TH * -1;
gosari 0:405f170eac1c 329 }
gosari 0:405f170eac1c 330
gosari 0:405f170eac1c 331 u = (u * -1) - 23; // offset correction value: 23
gosari 0:405f170eac1c 332
gosari 0:405f170eac1c 333 // Heave thruster output LPF part Start
gosari 0:405f170eac1c 334 if(firstRUN == 0)
gosari 0:405f170eac1c 335 {
gosari 0:405f170eac1c 336 prev_u = u;
gosari 0:405f170eac1c 337 firstRUN = 1;
gosari 0:405f170eac1c 338 }
gosari 0:405f170eac1c 339
gosari 0:405f170eac1c 340 curr_u = u;
gosari 0:405f170eac1c 341 u = int(D_alpha * float(prev_u) + (1.0 - D_alpha) * float(curr_u));
gosari 0:405f170eac1c 342 prev_u = u;
gosari 0:405f170eac1c 343 // Heave thruster output LPF part Finsh
gosari 0:405f170eac1c 344
gosari 0:405f170eac1c 345 c.TH_H(u);
gosari 0:405f170eac1c 346
gosari 0:405f170eac1c 347 if(can.write(CANMessage(16, (char*)c.Re(), 6)))
gosari 0:405f170eac1c 348 {
gosari 0:405f170eac1c 349 pc.printf("Depth PID test command sent!\r\n");
gosari 0:405f170eac1c 350 pc.printf("output: %d\r\n", u);
gosari 0:405f170eac1c 351 pc.printf("DEPTH: %d\r\n", DEPTH);
gosari 0:405f170eac1c 352 }
gosari 0:405f170eac1c 353
gosari 0:405f170eac1c 354 } // if d_flag
gosari 0:405f170eac1c 355 } // PID loop
gosari 0:405f170eac1c 356
gosari 0:405f170eac1c 357 stop = NULL;
gosari 0:405f170eac1c 358
gosari 0:405f170eac1c 359 } // stop 5
gosari 0:405f170eac1c 360
gosari 0:405f170eac1c 361 else if(stop == '8')
gosari 0:405f170eac1c 362 {
gosari 0:405f170eac1c 363 // IMU ON
gosari 0:405f170eac1c 364 pc.printf("Receiving IMU Data Start!\r\n");
gosari 0:405f170eac1c 365 pc.printf("To Finish, send 9\r\n");
gosari 0:405f170eac1c 366 while(1)
gosari 0:405f170eac1c 367 {
gosari 0:405f170eac1c 368 if(pc.readable())
gosari 0:405f170eac1c 369 {
gosari 0:405f170eac1c 370 if(pc.getc() == '9')
gosari 0:405f170eac1c 371 break;
gosari 0:405f170eac1c 372 }
gosari 0:405f170eac1c 373
gosari 0:405f170eac1c 374 BufferStart = 1;
gosari 0:405f170eac1c 375 BufferEnd = 0;
gosari 0:405f170eac1c 376
gosari 0:405f170eac1c 377 // Write Command
gosari 0:405f170eac1c 378 imu.putc(CommandByte);
gosari 0:405f170eac1c 379
gosari 0:405f170eac1c 380 do
gosari 0:405f170eac1c 381 {
gosari 0:405f170eac1c 382 Buffer[BufferEnd] = imu.getc();
gosari 0:405f170eac1c 383
gosari 0:405f170eac1c 384 //pc.putc(Buffer[BufferEnd]);
gosari 0:405f170eac1c 385 BufferEnd++;
gosari 0:405f170eac1c 386
gosari 0:405f170eac1c 387 BufferEnd %= PacketSize;
gosari 0:405f170eac1c 388 } while(imu.readable());
gosari 0:405f170eac1c 389
gosari 0:405f170eac1c 390 wait_ms(50);
gosari 0:405f170eac1c 391
gosari 0:405f170eac1c 392 // Convert Data
gosari 0:405f170eac1c 393 for (unsigned int i = 0; i < ResponseLength; i++)
gosari 0:405f170eac1c 394 {
gosari 0:405f170eac1c 395 for(unsigned int j = 3; j < 4; j--)
gosari 0:405f170eac1c 396 {
gosari 0:405f170eac1c 397 ((unsigned char*)&data[i])[j] = Buffer[BufferStart];
gosari 0:405f170eac1c 398 BufferStart++;
gosari 0:405f170eac1c 399 BufferStart %= PacketSize;
gosari 0:405f170eac1c 400 }
gosari 0:405f170eac1c 401 }
gosari 0:405f170eac1c 402 BufferStart += 6;
gosari 0:405f170eac1c 403 BufferStart %= PacketSize;
gosari 0:405f170eac1c 404
gosari 0:405f170eac1c 405 HEADING = (int)(data[2]*180.0/PI);
gosari 0:405f170eac1c 406
gosari 0:405f170eac1c 407 pc.printf("HEADING = %d\r\n", HEADING);
gosari 0:405f170eac1c 408
gosari 0:405f170eac1c 409 wait_ms(500);
gosari 0:405f170eac1c 410
gosari 0:405f170eac1c 411 } // IMU Data Receiving loop
gosari 0:405f170eac1c 412
gosari 0:405f170eac1c 413
gosari 0:405f170eac1c 414 stop = NULL;
gosari 0:405f170eac1c 415 } // stop 8
gosari 0:405f170eac1c 416
gosari 0:405f170eac1c 417
gosari 0:405f170eac1c 418
gosari 0:405f170eac1c 419 else
gosari 0:405f170eac1c 420 stop = NULL;
gosari 0:405f170eac1c 421
gosari 0:405f170eac1c 422 Thread::wait(100);
gosari 0:405f170eac1c 423
gosari 0:405f170eac1c 424 } // menu loop
gosari 0:405f170eac1c 425 }