File content as of revision 1:74aad7c18ccf:

/*
This is a program for globlelized controller of whole DOGO
F746ZG
*/

#include "mbed.h"
//#include "LSM9DS0_SH.h"
#define DEBUG       0

#define pi          3.141592f
#define d2r         0.01745329f

#define Rms         5000            //TT rate
#define dt          0.015f
#define Task_1_NN   120
#define Task_2_NN   5
#define Task_4_NN   9

//Structure
#define Buff_size   16              //Serial Buffer

//Initial Position
#define init_leg_Angle1 0
#define init_leg_length 0.22f
#define init_leg_Angle2 0

#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))

//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡GPIO registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
//╔═════════════════╗
//║    Structure    ║
//╚═════════════════╝
DigitalOut  LD1(PB_0);               //detection
DigitalOut  LD2(PB_7);               //detection
DigitalOut  LD3(PB_14);              //detection
InterruptIn button(USER_BUTTON);    //Button press;

//╔═════════════════╗
//║     Serial      ║
//╚═════════════════╝
Serial      Debug(PD_8, PD_9);      //Serial_3 reg(TX RX) USB port

Serial      LF_Cmd(PD_5, PD_6);     //Serial_2
DigitalOut  LF_CS(PD_7);

Serial      LH_Cmd(PC_12, PD_2);    //Serial_5
DigitalOut  LH_CS(PG_2);

Serial      RF_Cmd(PG_14, PC_7);    //Serial_6
DigitalOut  RF_CS(PG_9);

Serial      RH_Cmd(PE_8, PE_7);     //Serial_7
DigitalOut  RH_CS(PE_10);
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of GPIO registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Varible registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
//╔═════════════════╗
//║    Structure    ║
//╚═════════════════╝
Ticker  TT;                         //call a timer
uint8_t Task_1_count = 0;           //1st prior task count
uint8_t Task_2_count = 0;           //2nd prior task count
uint8_t Task_3_count = 0;           //3nd prior task count
uint8_t Task_4_count = 0;           //24nd prior task count
uint8_t Flag_1 = 0;                 //1st prior task flag
uint8_t Flag_2 = 0;                 //2nd prior task flag
uint8_t Flag_3 = 0;                 //3nd prior task flag
uint8_t Flag_4 = 0;                 //4nd prior task flag


//╔═════════════════╗
//║   I/O Serial    ║
//╚═════════════════╝
uint8_t Buff[Buff_size];
uint8_t Recieve_index = 0;


//╔═════════════════╗
//║   Leg Command   ║
//╚═════════════════╝

float   LF_q_0_E[3] = {            
    init_leg_Angle1, init_leg_length, init_leg_Angle2
};
float   LH_q_0_E[3] = {            
    init_leg_Angle1, init_leg_length, init_leg_Angle2
};
float   RF_q_0_E[3] = {            
    init_leg_Angle1, init_leg_length, init_leg_Angle2
};
float   RH_q_0_E[3] = {            
    init_leg_Angle1, init_leg_length, init_leg_Angle2
};

uint8_t LF_Cmd_Hex[3] = {0x00, 0x00, 0x00};
uint8_t LH_Cmd_Hex[3] = {0x00, 0x00, 0x00};
uint8_t RF_Cmd_Hex[3] = {0x00, 0x00, 0x00};
uint8_t RH_Cmd_Hex[3] = {0x00, 0x00, 0x00};


//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Varible registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Function registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void    init_TIMER();           //set TT_main() rate
void    TT_main();              //timebase function rated by TT
void    Rx_irq();
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Function registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡main funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
int main()
{
    Debug.baud(115200);                     //set baud rate
    LF_Cmd.baud(115200);
    LH_Cmd.baud(115200);
    RF_Cmd.baud(115200);
    RH_Cmd.baud(115200);

    wait_ms(10);
    init_TIMER();                           //start TT_main

    Debug.attach(&Rx_irq,Serial::RxIrq);    //Start recieving message
    
    LH_CS = 1;
    wait_ms(200);
    

    
    while(1) {                              //main() loop

//        if(Debug.readable()) {
//            Buff[Recieve_index] = Debug.getc();
//            Debug.putc(Buff[Recieve_index]);
//            Recieve_index = Recieve_index + 1;
//        }

//        if(device.readable()) {
//            pc.putc(device.getc());
//        }
        
        //Send commands
        if(Flag_4 == 1) {             
            
            LF_Cmd_Hex[0] = (LF_q_0_E[0]+pi/4)/(pi/2)*255;
            LF_Cmd_Hex[1] = ((LF_q_0_E[1]-0.1)/0.2)*255;
            LF_Cmd_Hex[2] = (LF_q_0_E[2]+pi/4)/(pi/2)*255;
            
            LH_Cmd_Hex[0] = (LH_q_0_E[0]+pi/4)/(pi/2)*255;
            LH_Cmd_Hex[1] = ((LH_q_0_E[1]-0.1)/0.2)*255;
            LH_Cmd_Hex[2] = (LH_q_0_E[2]+pi/4)/(pi/2)*255;
            
            RF_Cmd_Hex[0] = (RF_q_0_E[0]+pi/4)/(pi/2)*255;
            RF_Cmd_Hex[1] = ((RF_q_0_E[1]-0.1)/0.2)*255;
            RF_Cmd_Hex[2] = (RF_q_0_E[2]+pi/4)/(pi/2)*255;
            
            RH_Cmd_Hex[0] = (RH_q_0_E[0]+pi/4)/(pi/2)*255;
            RH_Cmd_Hex[1] = ((RH_q_0_E[1]-0.1)/0.2)*255;
            RH_Cmd_Hex[2] = (RH_q_0_E[2]+pi/4)/(pi/2)*255;
            
            //Debug.printf("%d, %d, %d\r", LH_Cmd_Hex[0], LH_Cmd_Hex[1], LH_Cmd_Hex[2]);
            //LED
            LD1 = 1;
            
            //Left Front Leg
            LF_CS = 0;
            wait_us(20);
            LF_Cmd.putc(LF_Cmd_Hex[0]);
            LF_Cmd.putc(LF_Cmd_Hex[1]);
            LF_Cmd.putc(LF_Cmd_Hex[2]);
            LF_Cmd.putc('Q');
            wait_us(180);
            LF_CS = 1;
            
            //Left Hind Leg
            LH_CS = 0;
            wait_us(20);
            LH_Cmd.putc(LH_Cmd_Hex[0]);
            LH_Cmd.putc(LH_Cmd_Hex[1]);
            LH_Cmd.putc(LH_Cmd_Hex[2]);
            LH_Cmd.putc('Q');
            wait_us(180);
            LH_CS = 1;
            
            //Right Front Leg
            RF_CS = 0;
            wait_us(20);
            RF_Cmd.putc(RF_Cmd_Hex[0]);
            RF_Cmd.putc(RF_Cmd_Hex[1]);
            RF_Cmd.putc(RF_Cmd_Hex[2]);
            RF_Cmd.putc('Q');
            wait_us(180);
            RF_CS = 1;
            
            //Right Hind Leg
            RH_CS = 0;
            wait_us(20);
            RH_Cmd.putc(RH_Cmd_Hex[0]);
            RH_Cmd.putc(RH_Cmd_Hex[1]);
            RH_Cmd.putc(RH_Cmd_Hex[2]);
            RH_Cmd.putc('Q');
            wait_us(180);
            RH_CS = 1;
            
            LD1 = 0;
            
            Flag_4 = 0;                 //clear pending
        }     
    }
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of main funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Timebase funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void init_TIMER()                       //set TT_main{} rate
{
    TT.attach_us(&TT_main, Rms);
}
void TT_main()                          //interrupt function by TT
{
    Task_1_count = Task_1_count + 1;
    if(Task_1_count > Task_1_NN) {
        Task_1_count = 0;               //Task triggering
        Flag_1 = 1;
    }
    Task_4_count = Task_4_count + 1;
    if(Task_4_count > Task_4_NN) {
        Task_4_count = 0;               //Task triggering
        Flag_4 = 1;
    }
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Timebase funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//


//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Rx_irq funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
void    Rx_irq(void)
{
    Buff[0] = Debug.getc();
    Debug.putc(Buff[0]);
}
//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Rx_irq funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//