File content as of revision 13:218c22a620cc:

#include "mbed.h"
#include "pin_config.h"

#include "communication.h"
#include "protocol.h"
#include "iSerial.h"

#include "motor_relay.h"
#include "motion_control.h"

//set frequancy unit in Hz
#define F_UPDATE 10.0f
#define TIMER_UPDATE 1.0f/F_UPDATE

//counter not receive from station
#define TIMEOUT_RESPONE_COMMAND 5

MOTION_CONTROL leg_left_upper(INA_L_U,INB_L_U,LIMIT_LU_U,LIMIT_LU_D,VR_LU);
MOTION_CONTROL leg_left_lower(INA_L_L,INB_L_L,LIMIT_LL_U,LIMIT_LL_D,VR_LL);
MOTION_CONTROL leg_right_upper(INA_R_U,INB_R_U,LIMIT_RU_U,LIMIT_RU_D,VR_RU);
MOTION_CONTROL leg_right_lower(INA_R_L,INB_R_L,LIMIT_RL_U,LIMIT_RL_D,VR_RL);

DigitalOut myled(LED1);
DigitalIn mybutton(USER_BUTTON);

//communication config
//serial for debug
Serial pc(USBTX, USBRX);
//serial for xbee
COMMUNICATION pan_a(TX_XBEE, RX_XBEE,115200,1000,1000); // tx, rx

//Fuction prototye
void getCommand();
//  init function
void calibration();
void test_position();
void test_status();
void test_limit();
void test_motor();

void routine();

void serial_control();

void management(ANDANTE_PROTOCOL_PACKET *packet);

void copy_data(ANDANTE_PROTOCOL_PACKET *scr, ANDANTE_PROTOCOL_PACKET *dst);


//set foreground
Ticker Update_command;

Timer t;

//struct
struct PARAM_WRITE {
    uint16_t left_up;
    uint16_t left_down;
    uint16_t right_up;
    uint16_t right_down;
};

//variable
//volatile ANDANTE_PROTOCOL_PACKET *param;
volatile uint8_t status=0;
//volatile PARAM_WRITE buff;
PARAM_WRITE buff;

int serial_data;

//Main function
int main()
{
    //int state=0;

    pc.baud(115200);
    pc.printf("Welcome to DOGWHEELSCHAIR\n");

    buff.left_up = 0;
    buff.right_up =0;
    buff.left_down = 64;
    buff.right_down = 64;

    if(mybutton == 0) {
        calibration();
    } else {
        pc.printf("Lock position Min-Max...");
        leg_left_upper.SetMaxPosition(63787);
        leg_left_upper.SetMinPosition(28653);

        leg_left_lower.SetMaxPosition(57609);
        leg_left_lower.SetMinPosition(17856);

        leg_right_upper.SetMaxPosition(52094);
        leg_right_upper.SetMinPosition(18928);

        leg_right_lower.SetMaxPosition(54383);
        leg_right_lower.SetMinPosition(8784);
        pc.printf("pass\n");
    }

    Update_command.attach(&getCommand,TIMER_UPDATE);

    serial_control();
    //test_motor();
    //test_limit();
    test_status();
    routine();
}

void getCommand()
{
    static uint8_t count =0;

    ANDANTE_PROTOCOL_PACKET packet;

    uint8_t status = pan_a.receiveCommunicatePacket(&packet);
    myled=0;

    if(status == ANDANTE_ERRBIT_NONE) {
        if(count >2 && count <10) {
            count--;
        } else {
            count=0;
        }


        //pan_a.sendCommunicatePacket(&packet);

        //update command
        //setControl(&packet);


    } else if(status == ANDANTE_ERRBIT_READ_TIMEOUT) {
        count++;
    }

    if(count >= TIMEOUT_RESPONE_COMMAND) {
        //stop robot
        count++;
        myled=1;
        //   setSpeedControl(0.0);
    }
}

void calibration()
{
    //calibration
    pc.printf("Calibration [START]\n");
    leg_left_upper.calibration();
    pc.printf("Left_UPPER\n");
    pc.printf("max_position = %d\n",leg_left_upper.GetMaxPosition());
    pc.printf("min_position = %d\n",leg_left_upper.GetMinPosition());
    leg_left_lower.calibration();
    pc.printf("Left_Lower\n");
    pc.printf("max_position = %d\n",leg_left_lower.GetMaxPosition());
    pc.printf("min_position = %d\n",leg_left_lower.GetMinPosition());
    leg_right_upper.calibration();
    pc.printf("right_UPPER\n");
    pc.printf("max_position = %d\n",leg_right_upper.GetMaxPosition());
    pc.printf("min_position = %d\n",leg_right_upper.GetMinPosition());
    leg_right_lower.calibration();
    pc.printf("right_Lower\n");
    pc.printf("max_position = %d\n",leg_right_lower.GetMaxPosition());
    pc.printf("min_position = %d\n",leg_right_lower.GetMinPosition());

    pc.printf("Calibration [FINISH]\n");
    pc.printf("RUN mode [START]\n");
    wait(1);
}

void test_position()
{
    uint8_t state;
    do {
        /*
        state=0;
        //leg_left_upper.position_control(500);
        if(leg_left_lower.position_control(500) ==2)
            state++;
        if(leg_right_upper.position_control(500) == 2)
            state++;
        if(leg_right_lower.position_control(500) == 2)
            state++;
        */
        state = leg_left_upper.position_control(32);
        pc.printf("state_lu %d\n",state);
        state = leg_left_lower.position_control(32);
        pc.printf("state_ll %d\n",state);
        state = leg_right_upper.position_control(32);
        pc.printf("state_ru %d\n",state);
        state = leg_right_lower.position_control(32);
        pc.printf("state_rl %d\n",state);
        state=0;
    } while(state != 2);
    pc.printf("[Finish test]\n");
}

void test_status()
{    
    uint16_t state=0;
    pc.printf("Test_status\n");
    t.start();
    while(1) {

        if(pc.readable() == 1) {
            serial_data = pc.getc();
        }

        if(t.read() > 10.0f) {
            t.reset();
            if(status >3) {
                status =0;
            } else {
                status++;
            }
        }

        if(status == 0) {
            state =0;
            // sleep
            state += leg_left_upper.position_control(0);
            state += leg_right_upper.position_control(0);
            state += leg_left_lower.position_control(64);
            state += leg_right_lower.position_control(64);
            if(state == 8) {
                myled=1;
            } else {
                myled=0;
            }
        } else if(status == 1 || status ==3) {
            state =0;
            // sit
            state += leg_left_upper.position_control(64);
            state += leg_right_upper.position_control(64);
            state += leg_left_lower.position_control(64);
            state += leg_right_lower.position_control(64);
            if(state == 8) {
                myled=1;
            } else {
                myled=0;
            }
        } else if(status == 2) {
            state =0;
            // stand
            state += leg_left_upper.position_control(64);
            state += leg_right_upper.position_control(64);
            state += leg_left_lower.position_control(0);
            state += leg_right_lower.position_control(0);
            if(state == 8) {
                myled=1;
            } else {
                myled=0;
            }
        }
    }
    //t.stop();
}

void management(ANDANTE_PROTOCOL_PACKET *packet)
{
    switch(packet->instructionErrorId) {
        case 0x01:
            buff.left_up = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
            break;
        case 0x02:
            buff.left_down = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
            break;
        case 0x03:
            buff.right_up = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
            break;
        case 0x04:
            buff.right_down = Utilities::ConvertUInt8ArrayToInt16(packet->parameter);
            break;
    }
}

void routine()
{
    uint8_t state=0;
//    PARAM_WRITE pos = buff;
    while(1) {
        state =0;
        // sit
        state += leg_left_upper.position_control(buff.left_up);
        state += leg_right_upper.position_control(buff.right_up);
        state += leg_left_lower.position_control(buff.left_down);
        state += leg_right_lower.position_control(buff.right_down);
        if(state == 8) {
            myled=1;
        } else {
            myled=0;
        }
    }
}

void test_limit()
{
    pc.printf("TEST LIMIT ALL\n");
    t.start();
    while(1) {
        if(t.read() > 1.0f) {
            t.reset();
            pc.printf("leftUP_limit_up = ");
            if(leg_left_upper.GetLimitUp() == 1)
                pc.printf("shot\n");
            else
                pc.printf("open\n");

            pc.printf("leftUP_limit_down = ");
            if(leg_left_upper.GetLimitDown() == 1)
                pc.printf("shot\n");
            else
                pc.printf("open\n");

            pc.printf("leftLOW_limit_up = ");
            if(leg_left_lower.GetLimitUp() == 1)
                pc.printf("shot\n");
            else
                pc.printf("open\n");

            pc.printf("leftLow_limit_down = ");
            if(leg_left_lower.GetLimitDown() == 1)
                pc.printf("shot\n");
            else
                pc.printf("open\n");

///////////////////////////////////////////////////////////////
            pc.printf("rightUP_limit_up = ");
            if(leg_right_upper.GetLimitUp() == 1)
                pc.printf("shot\n");
            else
                pc.printf("open\n");

            pc.printf("rightUP_limit_down = ");
            if(leg_right_upper.GetLimitDown() == 1)
                pc.printf("shot\n");
            else
                pc.printf("open\n");

            pc.printf("rightLOW_limit_up = ");
            if(leg_right_lower.GetLimitUp() == 1)
                pc.printf("shot\n");
            else
                pc.printf("open\n");

            pc.printf("rightLow_limit_down = ");
            if(leg_right_lower.GetLimitDown() == 1)
                pc.printf("shot\n");
            else
                pc.printf("open\n");

            pc.printf("\n\n");
        }
    }
}

void test_motor()
{
    uint8_t state=0;

    pc.printf("TEST MOTOR  DELAY\n");
    t.start();
    while(1) {
        if(t.read() > 1.5f) {
            t.reset();
            if(state >1) {
                state =0;
            } else {
                state++;
            }
        }

        if(state ==0) {
            leg_left_upper.limit_motor(1);
            leg_right_upper.limit_motor(1);
            leg_left_lower.limit_motor(1);
            leg_right_lower.limit_motor(1);
        } else {
            {
                leg_left_upper.limit_motor(2);
                leg_right_upper.limit_motor(2);
                leg_left_lower.limit_motor(2);
                leg_right_lower.limit_motor(2);
            }
        }
    }
}

void serial_control()
{

    uint16_t left_up=0;
    uint16_t left_down=0;
    uint16_t right_up=0;
    uint16_t right_down=0;

    uint16_t offset_ll=8700;
    uint16_t offset_rl=7500;

    uint8_t state=0;

    t.start();

    while(1) {
        if(pc.readable() == 1) {
            serial_data = pc.getc();
        }

        switch(serial_data) {
            case 'q':
                if(left_up >=64) {
                    left_up= 63;
                } else {
                    left_up++;
                }
         //       serial_data=0;
                break;

            case 'a':
                if(left_up >=64 || left_up <=0) {
                    left_up= 0;
                } else {
                    left_up--;
                }
         //       serial_data=0;
                break;

            case 'z':
                if(left_up >=54) {
                    left_up= 63;
                } else {
                    left_up+=10;
                }
        //        serial_data=0;
                break;

            case 'Z':
                if(left_up >=54 || left_up <=10) {
                    left_up= 0;
                } else {
                    left_up-=10;
                }
       //         serial_data=0;
                break;

            case 'w':
                if(left_down >=64) {
                    left_down= 63;
                } else {
                    left_down++;
                }
       //         serial_data=0;
                break;

            case 's':
                if(left_down >=64 || left_down <=0) {
                    left_down= 0;
                } else {
                    left_down--;
                }
        //        serial_data=0;
                break;

            case 'x':
                if(left_down >=54) {
                    left_down= 63;
                } else {
                    left_down+=10;
                }
     //           serial_data=0;
                break;

            case 'X':
                if(left_down >=54 || left_down <=10) {
                    left_down= 0;
                } else {
                    left_down-=10;
                }
//serial_data=0;
                break;


            case 'e':
                if(right_up >=64) {
                    right_up= 63;
                } else {
                    right_up++;
                }
       //         serial_data=0;
                break;

            case 'd':
                if(right_up >=64 || right_up <=0) {
                    right_up= 0;
                } else {
                    right_up--;
                }
       //         serial_data=0;
                break;

            case 'c':
                if(right_up >=54) {
                    right_up= 63;
                } else {
                    right_up+=10;
                }
     //           serial_data=0;
                break;

            case 'C':
                if(right_up >=54 || right_up <=10) {
                    right_up= 0;
                } else {
                    right_up-=10;
                }
     //           serial_data=0;
                break;




            case 'r':
                if(right_down >=64) {
                    right_down= 63;
                } else {
                    right_down++;
                }
                
                break;

            case 'f':
                if(right_down >=64 || right_down <=0) {
                    right_down= 0;
                } else {
                    right_down--;
                }
                
                break;

            case 'v':
                if(right_down >=54) {
                    right_down= 63;
                } else {
                    right_down+=10;
                }
  
                break;

            case 'V':
                if(right_down >=54 || right_down <=10) {
                    right_down= 0;
                } else {
                    right_down-=10;
                }
                
                break;

            case 'p':
                test_status();
                break;

            case 'y':
                if(offset_ll > 30000) {
                    offset_ll =30000;
                } else {

                    offset_ll+=100;
                }
                leg_left_lower.SetOffset(offset_ll);
                break;

            case 'h':
                if(offset_ll <= 100) {
                    offset_ll =0;
                } else {

                    offset_ll-=100;
                }
                leg_left_lower.SetOffset(offset_ll);
                break;

            case 'u':
                if(offset_rl > 30000) {
                    offset_rl =30000;
                } else {

                    offset_rl+=100;
                }
                leg_right_lower.SetOffset(offset_rl);
                break;

            case 'j':
                if(offset_rl <= 100) {
                    offset_rl =0;
                } else {

                    offset_rl-=100;
                }
                leg_right_lower.SetOffset(offset_rl);
                break;

            case '0':        
                leg_left_lower.SetMode(0);
                leg_right_lower.SetMode(0);
                pc.printf("MODE 0:ON\n");
                break;
            case '1':
                leg_left_lower.SetMode(1);
                leg_right_lower.SetMode(1);
                pc.printf("MODE 1:ON\n");
                break;
            case '2':
                leg_left_lower.SetMode(2);
                leg_right_lower.SetMode(2);
                pc.printf("MODE 2:ON\n");
                break;
        }

        if(t.read() > 1.0f) {
            t.reset();
            pc.printf("left_up %d\n",left_up);
            pc.printf("left_down %d\n",left_down);
            pc.printf("right_up %d\n",right_up);
            pc.printf("right_down %d\n",right_down);
            pc.printf("offset_ll %d\n",offset_ll);
            pc.printf("offset_rl %d\n",offset_rl);
            pc.printf("\n");

        }
         serial_data=0;
        state =0;

        state += leg_left_upper.position_control(left_up);
        state += leg_right_upper.position_control(right_up);
        state += leg_left_lower.position_control(left_down);
        state += leg_right_lower.position_control(right_down);
        if(state == 8) {
            myled=1;
        } else {
            myled=0;
        }
    }
}