File content as of revision 0:405f170eac1c:

#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    
}