d
Dependencies: Command mbed-rtos mbed
Revision 0:405f170eac1c, committed 2016-05-09
- Comitter:
- gosari
- Date:
- Mon May 09 06:08:14 2016 +0000
- Commit message:
- d
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Command.lib Mon May 09 06:08:14 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/gosari/code/Command/#9a3e048866cb
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon May 09 06:08:14 2016 +0000 @@ -0,0 +1,425 @@ +#include "mbed.h" +#include "CAN.h" +#include "Command.h" +#include "MS3DMGX2.h" +#include "rtos.h" + +#define PI 3.141592 + +typedef unsigned char BYTE; + +Serial pc(USBTX, USBRX); +CAN can(p30, p29); +DigitalOut led1(LED1); +Serial imu(p13, p14); + +int handle1; + +BYTE* Temp; + +int Read_Data[3] = {0,0,0}; +int DEPTH = 0; +int HEADING = 0; + +int flag_break; + +float data[6]; + +unsigned char Buffer[31]; +unsigned char PacketSize; +bool Checksum(unsigned char Index, unsigned char Length); + +unsigned char BufferStart; +unsigned char BufferEnd; +unsigned char CommandByte; +unsigned char ResponseLength; + + + +void imu_thread(void const *args){ + while(true){ + led1=!led1; + + //while(!IMU.Readable()); + //isvalid = IMU.Read(data); + + BufferStart = 1; + BufferEnd = 0; + + // Write Command + imu.putc(CommandByte); + + // Read + do + { + Buffer[BufferEnd] = imu.getc(); + + //pc.putc(Buffer[BufferEnd]); + BufferEnd++; + + BufferEnd %= PacketSize; + } while(imu.readable()); + + // Calculate Checksum + //unsigned short Sum = 0; + //for (unsigned char a = 0; a < PacketSize - 2; a++) + //{ + // Sum += Buffer[BufferStart]; + // BufferStart++; + // BufferStart %= PacketSize; + //} + //bool checksum = (((unsigned char*)&Sum)[0] == Buffer[BufferStart+1]) && (((unsigned char*)&Sum)[1] == Buffer[BufferStart]); + + //BufferStart = 1; + + for (unsigned int i = 0; i < ResponseLength; i++) + { + for(unsigned int j = 3; j < 4; j--) + { + ((unsigned char*)&data[i])[j] = Buffer[BufferStart]; + BufferStart++; + BufferStart %= PacketSize; + } + } + BufferStart += 6; + BufferStart %= PacketSize; + + + HEADING = (int)(data[2]*180.0/PI); // Heading = Yaw + + Thread::wait(500); + } +} + +int main(){ + + Command c; + + pc.baud(9600); + can.frequency(148148); + imu.baud(115200); + + CommandByte = 0xCF; + ResponseLength = 6; + PacketSize = 31; + + pc.printf("Connected to mbed..\r\n"); + + //Thread thread(imu_thread); + //thread.set_priority(osPriorityHigh); + pc.printf("IMU thread start..\r\n"); + + + + while(1) + { + pc.printf("-----------------------\r\n"); + pc.printf("All Stop = 0\r\n"); + pc.printf("Light On = 1\r\n"); + pc.printf("Heave = 2\r\n"); + pc.printf("Surge = 3\r\n"); + pc.printf("Heading PID_TEST = 4\r\n"); + pc.printf("Depth PID_TEST = 5\r\n"); + pc.printf("IMU Data Receive = 8\r\n"); + pc.printf("-----------------------\r\n"); + + + char stop = NULL; + stop = pc.getc(); + + if(stop == '0') + { + // All Stop + c.set(); + pc.printf("%s\r\n", (char*)c.Re()); + if(can.write(CANMessage(16, (char*)c.Re(), 6))) + { + pc.printf("All Stop command sent!\r\n"); + } + stop = NULL; + } + + else if(stop == '1') + { + // Light On + c.LIGHT(80); + pc.printf("%s\r\n", (char*)c.Re()); + if(can.write(CANMessage(16, (char*)c.Re(), 6))) + { + pc.printf("Light On command sent!\r\n"); + } + stop = NULL; + } + + else if(stop == '2') + { + // Heave + int HV; + pc.printf("input Heave value = "); + pc.scanf("%d", &HV); + c.TH_H(HV * -1); + pc.printf("%s\r\n", (char*)c.Re()); + if(can.write(CANMessage(16, (char*)c.Re(), 6))) + { + pc.printf("Heave %d command sent!\r\n", HV); + } + stop = NULL; + } + + else if(stop == '3') + { + // Surge + c.TH_L(10); + c.TH_R(10); + pc.printf("%s\r\n", (char*)c.Re()); + if(can.write(CANMessage(16, (char*)c.Re(), 6))) + { + pc.printf("Surge command sent!\r\n"); + } + stop = NULL; + } + + else if(stop == '4') + { + // PID_TEST + int T_HEADING = 0; // target heading value + pc.printf("input target heading (reference : %d) = ", HEADING); + pc.scanf("%d", &T_HEADING); + pc.printf("Press '9' to finish PID loop...\r\n"); + + int u = 0; // thruster input + float kp = 0.25; // gain + float ki = 0; // 0.005; + float kd = 0; // 0.25; + + int Max_TH = 30; // maximum TH output + int integral = 0; + + float E = 0; + float oldE = 0; + float oldEE = 0; + + int h_flag = 1; + while(1) + { + char a = NULL; + if(pc.readable()) + { + a = pc.getc(); + if(a == '9'){ + c.set(); + if(can.write(CANMessage(16, (char*)c.Re(), 6))) + { + pc.printf("PID Loop Stopped!\r\n"); + } + h_flag = 0; + break; + + } + } + + if(h_flag) + { + E = float(T_HEADING - HEADING); + + if(E < -180){ + E = E + 360; + } + + u = integral + int(kp*(E - oldE) + ki*E + kd*((E - oldE) - (oldE - oldEE))); + + oldEE = oldE; + oldE = E; + integral = u; + + if(u > 0){ + u = u + 6; + } + else if (u < 0){ + u = u - 6; + } + + if(u > Max_TH){ + u = Max_TH; + } + else if (u < Max_TH*-1){ + u = Max_TH * -1; + } + else if (u < 8 && u > -8){ + u = 0; + } + + c.TH_L(u*-1); + c.TH_R(u); + + if(can.write(CANMessage(16, (char*)c.Re(), 6))) + { + pc.printf("Heading PID test command sent!\r\n"); + pc.printf("output: %d\r\n", u); + pc.printf("error: %d\r\n", T_HEADING - HEADING); + } + + + } // if h_flag + } // PID loop + + stop = NULL; + + } // stop 4 + + else if(stop == '5') + { + // PID_TEST + int T_DEPTH = 0; // target depth value + pc.printf("input target depth (reference : %d) = ", DEPTH); + pc.scanf("%d", &T_DEPTH); + pc.printf("Press '9' to finish PID loop...\r\n"); + + int u = 0; // thruster input + float kp = 1; // 1.0 + float ki = 0; // 0.005; + float kd = 0.1; // 0.25; + + int Max_TH = 60; // maximum TH output + int integral = 0; + + float E = 0; + float oldE = 0; + float oldEE = 0; + + int firstRUN = 0; + float D_alpha = 0.7; + int prev_u = 0; + int curr_u = 0; + + int d_flag = 1; + while(1) + { + if(pc.readable()) + { + char a = NULL; + a = pc.getc(); + if(a == '9') + { + c.set(); + if(can.write(CANMessage(16, (char*)c.Re(), 6))) + { + pc.printf("PID Loop Stopped!\r\n"); + } + d_flag = 0; + break; + } + } + + if(d_flag) + { + E = float(T_DEPTH - DEPTH); + + u = integral + int(kp*(E - oldE) + ki*E + kd*((E - oldE) - (oldE - oldEE))); + + oldEE = oldE; + oldE = E; + integral = u; + + if(u > Max_TH){ + u = Max_TH; + } + else if (u < Max_TH*-1){ + u = Max_TH * -1; + } + + u = (u * -1) - 23; // offset correction value: 23 + + // Heave thruster output LPF part Start + if(firstRUN == 0) + { + prev_u = u; + firstRUN = 1; + } + + curr_u = u; + u = int(D_alpha * float(prev_u) + (1.0 - D_alpha) * float(curr_u)); + prev_u = u; + // Heave thruster output LPF part Finsh + + c.TH_H(u); + + if(can.write(CANMessage(16, (char*)c.Re(), 6))) + { + pc.printf("Depth PID test command sent!\r\n"); + pc.printf("output: %d\r\n", u); + pc.printf("DEPTH: %d\r\n", DEPTH); + } + + } // if d_flag + } // PID loop + + stop = NULL; + + } // stop 5 + + else if(stop == '8') + { + // IMU ON + pc.printf("Receiving IMU Data Start!\r\n"); + pc.printf("To Finish, send 9\r\n"); + while(1) + { + if(pc.readable()) + { + if(pc.getc() == '9') + break; + } + + BufferStart = 1; + BufferEnd = 0; + + // Write Command + imu.putc(CommandByte); + + do + { + Buffer[BufferEnd] = imu.getc(); + + //pc.putc(Buffer[BufferEnd]); + BufferEnd++; + + BufferEnd %= PacketSize; + } while(imu.readable()); + + wait_ms(50); + + // Convert Data + for (unsigned int i = 0; i < ResponseLength; i++) + { + for(unsigned int j = 3; j < 4; j--) + { + ((unsigned char*)&data[i])[j] = Buffer[BufferStart]; + BufferStart++; + BufferStart %= PacketSize; + } + } + BufferStart += 6; + BufferStart %= PacketSize; + + HEADING = (int)(data[2]*180.0/PI); + + pc.printf("HEADING = %d\r\n", HEADING); + + wait_ms(500); + + } // IMU Data Receiving loop + + + stop = NULL; + } // stop 8 + + + + else + stop = NULL; + + Thread::wait(100); + + } // menu loop +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Mon May 09 06:08:14 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#162b12aea5f2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon May 09 06:08:14 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/99a22ba036c9 \ No newline at end of file