Seokyong Song
d
Dependencies: Command mbed-rtos mbed
Diff: main.cpp
- Revision:
- 0:405f170eac1c
diff -r 000000000000 -r 405f170eac1c main.cpp
--- /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
+}