File content as of revision 1:45f1573d65a1:

//*****************************************************/
//  Include  //
#include "mbed.h"
#include "pinconfig.h"
#include "PID.h"
//#include "Motor.h"
#include "eeprom.h"
#include "Receiver.h"
#include "Motion_EEPROM_Address.h"
#include "move.h"
#include "UNTRASONIC.h"

//#include "pidcontrol.h"

#define EEPROM_DELAY 2
DigitalOut rs485_dirc1(RS485_DIRC);
//#define DEBUG_UP
//#define DEBUG_LOW

InterruptIn encoderA_d(PB_12);
DigitalIn encoderB_d(PB_13);
InterruptIn encoderA_1(PB_1);
DigitalIn encoderB_1(PB_2);
InterruptIn encoderA_2(PB_14);
DigitalIn encoderB_2(PB_15);
DigitalOut rs485_dirc1(RS485_DIRC);
Timer timerStart;
Timeout time_getsensor;
Timeout time_distance;
Timeout shutdown;
move m1;
//*****************************************************/
//--PID parameter--
//-Upper-//
float U_Kc=0.2;
float U_Ki_gain=0.0;
float U_Kd_gain=0.0;
float U_Ti=0.0;
float U_Td=0.0;
float U_Ki=U_Kc*U_Ti;
float U_Kd=U_Kc*U_Td;
//-lower-//
float L_Kc=0.2;
float L_Ki_gain=0.0;
float L_Kd_gain=0.0;
float L_Ti=0.0;
float L_Td=0.0;
float L_Ki=L_Kc*L_Ti;
float L_Kd=L_Kc*L_Td;
//*****************************************************/
// Global  //
//timer
 int timer_now=0,timer_later=0;
 int times=0,timer_buffer=0;
 
 //encoder
 int Encoderpos = 0;
 int real_d=0;
 float valocity1 =0,valocity2 =0,pulse_1=0,pulse_2=0,count=0,r=0.125,velocityreal=0,pulse_d=0,Z_d=0;
//pid

double setp1=0,setp2=0;
float outPID =0;
float VRmax,VLmax,VR,VL,KP_LEFT,KI_LEFT,KD_LEFT,KP_RIGHT,KI_RIGHT ,KD_RIGHT ;
PID P1(KP_LEFT,KI_LEFT,KD_LEFT,0.1);
PID P2(KP_RIGHT,KI_RIGHT ,KD_RIGHT,0.1);
//Ticker Recieve;
//-- Communication --
COMMUNICATION *com1;
Serial PC(SERIAL_TX,SERIAL_RX);
Bear_Receiver com(SERIAL_TX,SERIAL_RX,115200);
int16_t MY_ID = 0x00;
//-- Memorry --
EEPROM memory(PB_4,PA_8,0);

//-- encoder --

//-- Motor --
//int dir;
//Motor Upper(PWM_LU,A_LU,B_LU);
//Motor Lower(PWM_LL,A_LL,B_LL);
//-- PID --
//int Upper_SetPoint=20;
//int Lower_SetPoint=8;
//PID Up_PID(U_Kc, U_Ti, U_Td, 0.001);//Kp,Ki,Kd,Rate
//PID Low_PID(L_Kc, L_Ti, L_Td, 0.001);

//PID Up_PID(U_Kc, U_Ti, U_Td);//Kp,Ki,Kd,Rate
//PID Low_PID(L_Kc, L_Ti, L_Td);

//*****************************************************/
//void Start_Up();
void CmdCheck(int16_t id,uint8_t *command,uint8_t ins);

//Timer counterUP;
//Timer counterLOW;

DigitalOut myled(LED1);


void Rx_interrupt()
{
    //s1.get_motor();รับค่ามอเตอร์
    RC();
    timer_later= timer_now;
  
}
void EncoderA_1()//ซ้าย
{   if(encoderB_1==0)
        { Encoderpos = Encoderpos + 1;}
    else
   { Encoderpos = Encoderpos -1;}
   pulse_1+=1;
   //Encoderpos = Encoderpos + 1;
   //valocity+=1;
   //pc.printf("%d \n",Encoderpos);
   //pc.printf("pulse=%f  \n",pulse);
   //if(pulse==128)
   //{count+=1;pulse=0; pc.printf("count=%f  \n",count);}
}
  void EncoderA_2()//ขวา
{ 
    if(encoderB_2==0)
    { Encoderpos = Encoderpos + 1;}
    else
    { Encoderpos = Encoderpos -1;}
    pulse_2+=1;
    //pc.printf("%d",Encoderpos);
}
void EncoderA_D()
{ 
    if(encoderB_d==0)
    { Encoderpos = Encoderpos + 1;}
    else
    { Encoderpos = Encoderpos -1;}
    pulse_d+=1;
    if(pulse_d==128)
    {
        Z_d+=1;
        pulse_d=0;
    }
   // pc.printf("%d",Encoderpos);
}
void getvelo1()//จาก encoder
{
    valocity1=pulse_1*((2*3.14*r)/128);
    pc.printf("valocity=%f  \n",valocity1);
    count=0;
    timerStart.reset();
}
void getvelo2()
{
    valocity2=pulse_2*((2*3.14*r)/128);
    pc.printf("valocity=%f  \n",valocity2);
    count=0;
    timerStart.reset();
}
void get_d()//ระยะทาง
{
    real_d=Z_d*(2*3.14*r);
    //ส่งข้อมูล
    
}
double map(double x, double in_min, double in_max, double out_min, double out_max)
{
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
    
}
void PID_m1()//left
{
    setp1=map(1.0,0.0,1.094,0.0,1.0);
    P1.setSetPoint(setp1);
     times=timerStart.read();
       if(times==1)// m/s
       {   
           getvelo1();
           //pc.printf("TIME \n");
           times=0;
           pulse_1=0;
        }
    P1.setProcessValue(valocity1);
    outPID=P1.compute();
     //pc.printf("outPID=%f \n",outPID);
     m1.movespeed_1(setp1,outPID);
}
void PID_m2()//right
{
    setp2=map(1.0,0.0,1.094,0.0,1.0);
    P2.setSetPoint(setp2);
     times=timerStart.read();
       if(times==1)// m/s
       {   
           getvelo2();
           //pc.printf("TIME \n");
           times=0;
           pulse_2=0;
        }
    P2.setProcessValue(valocity2);
    outPID=P2.compute();
     //pc.printf("outPID=%f \n",outPID);
     m1.movespeed_2(1,setp2,outPID);
}
/*
void Read_Encoder(PinName Encoder)
{
    ENC.format(8,0);
    ENC.frequency(200000);//due to rising time,have to decrease clock from 1M - 240k

    if(Encoder == EncoderA) {
        EncA = 0;
    } else {
        EncB = 0;
    }
    ENC.write(0x41);
    ENC.write(0x09);
    data = ENC.write(0x00);
    if(Encoder == EncoderA) {
        EncA = 1;
    } else {
        EncB = 1;
    }

}
//****************************************************
int Get_EnValue(int Val)
{
    int i = 0;
    static unsigned char codes[] = {
        127, 63, 62, 58, 56, 184, 152, 24, 8, 72, 73, 77, 79, 15, 47, 175,
        191, 159, 31, 29, 28, 92, 76, 12, 4, 36, 164, 166, 167, 135, 151, 215,
        223, 207, 143, 142, 14, 46, 38, 6, 2, 18, 82, 83, 211, 195, 203, 235,
        239, 231, 199, 71, 7, 23, 19, 3, 1, 9, 41, 169, 233, 225, 229, 245,
        247, 243, 227, 163, 131, 139, 137, 129, 128, 132, 148, 212, 244, 240, 242, 250,
        251, 249, 241, 209, 193, 197, 196, 192, 64, 66, 74, 106, 122, 120, 121, 125,
        253, 252, 248, 232, 224, 226, 98, 96, 32, 33, 37, 53, 61, 60, 188, 190,
        254, 126, 124, 116, 112, 113, 49, 48, 16, 144, 146, 154, 158, 30, 94, 95
    };
    if ( MY_ID == 0x01 ) { //when it was left side
        while(Val != codes[i]) {
            i++;
        }
    }
    if ( MY_ID == 0x02 ) { //when it was right side
        while(Val != codes[127-i]) {
            i++;
        }
    }
    return i;

}
//****************************************************
void SET_UpperPID()
{
    Upper.period(0.001);

    memory.read(ADDRESS_UP_MARGIN,UpMargin);
    Up_PID.setMargin(UpMargin);

    memory.read(ADDRESS_UPPER_KP,U_Kc);
    Up_PID.setKp(U_Kc);
    memory.read(ADDRESS_UPPER_KI,U_Ki_gain);
    Up_PID.setKi(U_Ki_gain);
    memory.read(ADDRESS_UPPER_KD,U_Kd_gain);
    Up_PID.setKd(U_Kd_gain);

    //Up_PID.setMode(0);
    //Up_PID.setInputLimits(18,62);
    //Up_PID.setOutputLimits(0,1);
}
//******************************************************/
/*
void SET_LowerPID()
{
    Lower.period(0.001);

    memory.read(ADDRESS_LOW_MARGIN,LowMargin);
    Low_PID.setMargin(LowMargin);

    memory.read(ADDRESS_LOWER_KP,L_Kc);
    Low_PID.setKp(L_Kc);
    memory.read(ADDRESS_LOWER_KI,L_Ki_gain);
    Low_PID.setKi(L_Ki_gain);

    memory.read(ADDRESS_LOWER_KD,L_Kd_gain);
    Low_PID.setKd(L_Kd_gain);

    //Low_PID.setMode(0);
    //Low_PID.setInputLimits(0,127); // set range
    //Low_PID.setOutputLimits(0,1);
}
//******************************************************/
/*
void Move_Upper()
{
    Read_Encoder(EncoderA);
    Upper_Position = (float)Get_EnValue(data);
#ifdef DEBUG_UP
    printf("read_encode = 0x%2x \n\r",data);
    printf("Setpoint = %d\n\r",Upper_SetPoint);
    printf("Upper_Position = %f\n\r",Upper_Position);
#endif
    Up_PID.setCurrent(Upper_Position);
    float speed =Up_PID.compute();
#ifdef DEBUG_UP
    printf("E_n= %f\n\r",Up_PID.getErrorNow());
    printf("Kp = %f\n\r",Up_PID.getKp());
    printf("speed = %f\n\n\n\r",speed);
#endif
    Upper.speed(speed);
}
//******************************************************/
/*
void Move_Lower()
{
    Read_Encoder(EncoderB);
    Lower_Position = (float)Get_EnValue(data);
    Low_PID.setCurrent(Lower_Position);
#ifdef DEBUG_LOW
    printf("read_encode = 0x%2x \n\r",data);
    printf("Setpoint = %d\n\r",Lower_SetPoint);
    printf("Upper_Position = %f\n\r",Lower_Position);
#endif

    float speed =Low_PID.compute();
#ifdef DEBUG_LOW
    printf("E_n= %f\n\r",Low_PID.getErrorNow());
    printf("Kp = %f\n\r",Low_PID.getKp());
    printf("speed = %f\n\n\n\r",speed);
#endif
    Lower.speed(speed);
}
//******************************************************/


void Rc()
{
    myled =1;
    uint8_t data_array[30];
    uint8_t id=0;
    uint8_t ins=0;
    uint8_t status=0xFF;



    status = com.ReceiveCommand(&id,data_array,&ins);
    PC.printf("status = 0x%02x\n\r",status);
    if(status == ANDANTE_ERRBIT_NONE) {
        CmdCheck((int16_t)id,data_array,ins);
        PC.printf("s******************************");
    }

}
/*******************************************************/
int main()
{
    PC.baud(115200);
    printf("******************");
    /*
    while(1)
    {
    Read_Encoder(EncoderA);
    Upper_Position = Get_EnValue(data);
    Read_Encoder(EncoderB);
    Lower_Position = Get_EnValue(data);
    PC.printf("Upper Position : %f\n",Upper_Position);
    PC.printf("Lower_Position : %f\n",Lower_Position);
    wait(0.5);
    }
    */


    //Recieve.attach(&Rc,0.025);
    //Start_Up();

    //SET_UpperPID();
   // SET_LowerPID();

   // printf("BEAR MOTION ID:0x%02x\n\r",MY_ID);
    /*
    while(1)
    {

        Upper.speed(-1);
        wait_ms(400);
        Upper.speed(1);
        wait_ms(400);
        Upper.break();

        Lower.speed(-1.0);
        wait_ms(401);
        Lower.speed(1.0);
        wait_ms(401);
        Lower.break();
        }
    */

    //  counterUP.start();
//    counterLOW.start();

    while(1) {

        /*
        //printf("timer = %d\n\r",counter.read_ms());
        if(counterUP.read_ms() > 1400) {

            if(Upper_SetPoint < 53) {
                Upper_SetPoint++;
            } else
                Upper_SetPoint =18;

            counterUP.reset();

        }

        if(counterLOW.read_ms() > 700) {

            if(Lower_SetPoint < 100) {
                Lower_SetPoint++;
            } else
                Lower_SetPoint =8;

            counterLOW.reset();

        }
        */
      //  myled =1;
        //wait_ms(10);
///////////////////////////////////////////////////// start
        //Set Set_point
       // Up_PID.setGoal(Upper_SetPoint);
       // Low_PID.setGoal(Lower_SetPoint);

        //Control Motor
       // Move_Upper();
       // Move_Lower();
/////////////////////////////////////////////////////  stop =306us
        //uint8_t aaa[1]={10};
        //com.sendBodyWidth(0x01,aaa);
        Rc();
        //wait_ms(1);
    }
}









void CmdCheck(int16_t id,uint8_t *command,uint8_t ins)
{
       PC.printf("cmdcheck\n");
     if(id==MY_ID) {
        switch (ins) {
            case PING: {
                break;
            }
            case WRITE_DATA: {
                switch (command[0]) {
                    case ID: {
                        ///
                        MY_ID = (int16_t)command[1];
                        break;
                    }
                    case SET_VELOCITY_LEFT: {
                        //
                        uint8_t int_buffer[2];
                        float Int;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                        VL=Int/1000;
                        PC.printf("VL=%f /n",VL);
                        break;
                    }
                    case SET_VELOCITY_RIGHT: {
                        //
                        uint8_t int_buffer[2];
                        float Int;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                        VR=Int/1000;
                        break;
                    }
                    case SET_VELOCITY_MAX_LEFT: {
                        //
                        uint8_t int_buffer[2];
                        float Int;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                   VLmax=Int/1000;
                        break;
                    }
                    case SET_VELOCITY_MAX_RIGHT: {
                        //
                        uint8_t int_buffer[2];
                        float Int;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                        VRmax=Int/1000;
                        PC.printf("VRmax = %f",VRmax);
                        PC.printf("*****************************");
                        break;
                    }
                    case SET_KP_LEFT: {
                        uint8_t int_buffer[2];
                        float Int;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                       KP_LEFT=Int/1000;
                        break;
                    }
                    case SET_KI_LEFT: {
                       uint8_t int_buffer[2];
                        float Int;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                       KI_LEFT=Int/1000;
                        break;
                    }
                    case SET_KD_LEFT: {
                        uint8_t int_buffer[2];
                        float Int;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                       KD_LEFT=Int/1000;
                        break;
                    }
                    case  SET_KP_RIGHT: {
                       uint8_t int_buffer[2];
                        float Int;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                       KP_RIGHT=Int/1000;
                        break;
                    }
                    case SET_KI_RIGHT: {
                       uint8_t int_buffer[2];
                        float Int;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                       KI_RIGHT=Int/1000;
                        break;
                    }
                    case SET_KD_RIGHT: {
                        uint8_t int_buffer[2];
                        float Int;
                        int_buffer[0]=command[1];
                        int_buffer[1]=command[2];
                        Int=(float)Utilities::ConvertUInt8ArrayToInt16(int_buffer);
                       KD_RIGHT=Int/1000;
                        break;
                    }
                 }
            } break;
             case READ_DATA: {
                switch (command[0]) {
                    case GET_LIDAR: {
                        
                        break;
                        }
                    case GET_BATTERY: {
                        
                        break;
                        }
                    case GET_VELOCITY_LEFT: {
                        uint8_t intVelo_L[2],floatVelo_L[2];
                        com.FloatSep(valocity1,intVelo_L,floatVelo_L);
                        
                        
                         ANDANTE_PROTOCOL_PACKET package;

                        package.robotId = MY_ID;
                         package.length = 6;
                        package.instructionErrorId = WRITE_DATA;
                        package.parameter[0]=intVelo_L[0];
                        package.parameter[1]=intVelo_L[1];
                         package.parameter[2]=floatVelo_L[0];
                        package.parameter[3]=floatVelo_L[1];

                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        
                        break;
                        }
                    case GET_VELOCITY_RIGHT : {
                        uint8_t intVelo_R[2],floatVelo_R[2];
                        com.FloatSep(valocity2,intVelo_R,floatVelo_R);
                        
                        
                         ANDANTE_PROTOCOL_PACKET package;

                        package.robotId = MY_ID;
                         package.length = 6;
                        package.instructionErrorId = WRITE_DATA;
                        package.parameter[0]=intVelo_R[0];
                        package.parameter[1]=intVelo_R[1];
                         package.parameter[2]=floatVelo_R[0];
                        package.parameter[3]=floatVelo_R[1];

                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        break;
                        }
                    case GET_KP_LEFT: {
                        uint8_t intKPL[2],floatKPL[2];
                        com.FloatSep(KP_LEFT,intKPL,floatKPL);
                        
                        
                         ANDANTE_PROTOCOL_PACKET package;

                        package.robotId = MY_ID;
                         package.length = 6;
                        package.instructionErrorId = WRITE_DATA;
                        package.parameter[0]=intKPL[0];
                        package.parameter[1]=intKPL[1];
                         package.parameter[2]=floatKPL[0];
                        package.parameter[3]=floatKPL[1];

                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        break;
                        }
                    case GET_KI_LEFT: {
                        uint8_t intKIL[2],floatKIL[2];
                        com.FloatSep(KI_LEFT,intKIL,floatKIL);
                        
                        
                         ANDANTE_PROTOCOL_PACKET package;

                        package.robotId = MY_ID;
                         package.length = 6;
                        package.instructionErrorId = WRITE_DATA;
                        package.parameter[0]=intKIL[0];
                        package.parameter[1]=intKIL[1];
                         package.parameter[2]=floatKIL[0];
                        package.parameter[3]=floatKIL[1];

                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        break;
                        }
                    case GET_KD_LEFT: {
                        uint8_t intKDL[2],floatKDL[2];
                        com.FloatSep(KD_LEFT,intKDL,floatKDL);
                        
                        
                         ANDANTE_PROTOCOL_PACKET package;

                        package.robotId = MY_ID;
                         package.length = 6;
                        package.instructionErrorId = WRITE_DATA;
                        package.parameter[0]=intKDL[0];
                        package.parameter[1]=intKDL[1];
                         package.parameter[2]=floatKDL[0];
                        package.parameter[3]=floatKDL[1];

                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        break;
                        }
                    case GET_KP_RIGHT: {
                        uint8_t intKDR[2],floatKDR[2];
                        com.FloatSep(KP_RIGHT,intKDR,floatKDR);
                        
                        
                         ANDANTE_PROTOCOL_PACKET package;

                        package.robotId = MY_ID;
                         package.length = 6;
                        package.instructionErrorId = WRITE_DATA;
                        package.parameter[0]=intKDR[0];
                        package.parameter[1]=intKDR[1];
                         package.parameter[2]=floatKDR[0];
                        package.parameter[3]=floatKDR[1];

                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        break;
                        }
                    case GET_KI_RIGHT: {
                        uint8_t intKIR[2],floatKIR[2];
                        com.FloatSep(KI_RIGHT,intKIR,floatKIR);
                        
                        
                         ANDANTE_PROTOCOL_PACKET package;

                        package.robotId = MY_ID;
                         package.length = 6;
                        package.instructionErrorId = WRITE_DATA;
                        package.parameter[0]=intKIR[0];
                        package.parameter[1]=intKIR[1];
                         package.parameter[2]=floatKIR[0];
                        package.parameter[3]=floatKIR[1];

                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        break;
                        }
                    case GET_KD_RIGHT: {
                        uint8_t intKDR[2],floatKDR[2];
                        com.FloatSep(KD_RIGHT,intKDR,floatKDR);
                        
                        
                         ANDANTE_PROTOCOL_PACKET package;

                        package.robotId = MY_ID;
                         package.length = 6;
                        package.instructionErrorId = WRITE_DATA;
                        package.parameter[0]=intKDR[0];
                        package.parameter[1]=intKDR[1];
                         package.parameter[2]=floatKDR[0];
                        package.parameter[3]=floatKDR[1];

                        rs485_dirc1=1;
                         wait_us(RS485_DELAY);
                         com1->sendCommunicatePacket(&package);
                        
                        break;
                        }
           }
     }break;
                    
    }
  }
}