File content as of revision 11:3dd92d1d542c:

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

//*****************************************************/
//--PID parameter--
//-Upper-//
float U_Kc;
float U_Ti;
float U_Td;
//-lower-//
float L_Kc;
float L_Ti;
float L_Td;

//*****************************************************/
// Global  //
Ticker Recieve;
//-- Communication --
Serial PC(D1,D0);
Bear_Receiver com(Tx,Rx,115200);
int8_t MY_ID = 0x01;
//-- Memorry --
EEPROM memory(PB_4,PA_8,0);
//-- encoder --
int Upper_Position;
int Lower_Position;
int data;
SPI ENC(Emosi, Emiso, Esck);
DigitalOut EncA(EncoderA);
DigitalOut EncB(EncoderB);
//-- Motor --
int dir;
Motor Upper(PWM_LU,A_LU,B_LU);
Motor Lower(PWM_LL,A_LL,B_LL);
//-- PID --
int Upper_SetPoint;
int Lower_SetPoint;
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);
//*****************************************************/
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
    };

    while(Val != codes[i]) {
        i++;
    }

    return i;

}
//*****************************************************/
void SET_UpperPID()
{
    Upper.period(0.001);
    Up_PID.setMode(0);
    Up_PID.setInputLimits(0,127);
    Up_PID.setOutputLimits(0,1);
}
//******************************************************/
void SET_LowerPID()
{
    Lower.period(0.001);
    Low_PID.setMode(0);
    Low_PID.setInputLimits(0,127); // set range
    Low_PID.setOutputLimits(0,1);
}
//******************************************************/
void Move_Upper()
{
    Read_Encoder(EncoderA);
    Upper_Position = Get_EnValue(data);

    Up_PID.setProcessValue(Upper_Position);

    if(Upper_Position - Upper_SetPoint > 0 ) {
        dir = 1;
    }
    if(Upper_Position - Upper_SetPoint < 0 ) {
        dir = -1;
    }
    Upper.speed(Up_PID.compute() * dir);
}
//******************************************************/
void Move_Lower()
{
    Read_Encoder(EncoderB);
    Lower_Position = Get_EnValue(data);

    Low_PID.setProcessValue(Lower_Position);

    if(Lower_Position - Lower_SetPoint > 0 ) {
        dir = 1;
    }
    if(Lower_Position - Lower_SetPoint < 0 ) {
        dir = -1;
    }
    Lower.speed(Low_PID.compute() * dir);
}
//******************************************************/

void CmdCheck(uint8_t *cmd,uint8_t ins)
{
    switch(cmd[0]) {
        case SET_MOTOR_UPPER_ANG : {
            uint8_t Upper_setpoint_buffer[2];
            uint8_t Lower_setpoint_buffer[2];
            Upper_setpoint_buffer[0]=cmd[1];
            Upper_setpoint_buffer[1]=cmd[2];
            Upper_SetPoint=Utilities::ConvertUInt8ArrayToInt16(Upper_setpoint_buffer);
            Lower_setpoint_buffer[0]=cmd[5];
            Lower_setpoint_buffer[1]=cmd[6];
            Lower_SetPoint=Utilities::ConvertUInt8ArrayToInt16(Lower_setpoint_buffer);
            //Set Set_point
            Up_PID.setSetPoint(Upper_SetPoint);
            Low_PID.setSetPoint(Lower_SetPoint);

            //Control Motor
            Move_Upper();
            Move_Lower();
        }

        case GET_MOTOR_UPPER_ANG : {
            float up_angle,low_angle;
            //
            com.sendMotorPos(MY_ID,up_angle,low_angle);
        }
        case SAVE_DATA_TO_EEPROM : {
            uint8_t Int_data_buffer[2];
            uint8_t Float_data_buffer[2];
            uint8_t Address_buffer[2];
            float int_buffer;
            float float_buffer;
            float data;
            uint16_t address;
            Int_data_buffer[0]=cmd[1];
            Int_data_buffer[1]=cmd[2];
            Float_data_buffer[0]=cmd[3];
            Float_data_buffer[1]=cmd[4];
            int_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(Int_data_buffer);
            float_buffer=(float)Utilities::ConvertUInt8ArrayToInt16(Float_data_buffer)/FLOAT_CONVERTER;
            
            address=Utilities::ConvertUInt8ArrayToInt16(Address_buffer);
            data=int_buffer+float_buffer;
            
            //
        }
    }
}
/******************************************************/
void Start_Up ()// load parameter from eeprom before start
{
    // wait for reciever
    memory.read(0x00,MY_ID);
    memory.read(0x04,Upper_SetPoint);
    memory.read(0x08,Lower_SetPoint);
    memory.read(0x10,U_Kc);
    memory.read(0x14,U_Ti);
    memory.read(0x18,U_Td);
    memory.read(0x1c,L_Kc);
    memory.read(0x20,L_Ti);
    memory.read(0x24,L_Td);
    
}
/*******************************************************/    
void Rc ()
{
    uint8_t data_array[10];
    int8_t ins;
    
    com.ReceiveCommand(MY_ID,data_array);
    CmdCheck(data_array,ins);
}
/*******************************************************/
int main()
{
    Recieve.attach(&Rc,0.2);
    Start_Up();
    SET_UpperPID();
    SET_LowerPID();

        while(1) {
            //Set Set_point
            Up_PID.setSetPoint(Upper_SetPoint);
            Low_PID.setSetPoint(Lower_SetPoint);

            //Control Motor
            Move_Upper();
            Move_Lower();
        }


    //com.ReceiveCommand(id,data_array);
}