![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |