File content as of revision 11:336dd293daa1:

#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
iSerial 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 copy_data(ANDANTE_PROTOCOL_PACKET *scr, ANDANTE_PROTOCOL_PACKET *dst);


//set foreground
Ticker Update_command;

Timer t;

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

//Main function
int main()
{
    int state=0;
    pc.baud(115200);
    pc.printf("Welcome to DOGWHEELSCHAIR\n");
    if(mybutton == 0) {
        calibration();
    } else {
        pc.printf("Lock position Min-Max...");
        leg_left_upper.SetMaxPosition(56244);
        leg_left_upper.SetMinPosition(20806);

        leg_left_lower.SetMaxPosition(50996);
        leg_left_lower.SetMinPosition(5371);

        leg_right_upper.SetMaxPosition(38985);
        leg_right_upper.SetMinPosition(8545);

        leg_right_lower.SetMaxPosition(38027);
        leg_right_lower.SetMinPosition(40);
        pc.printf("pass\n");
    }

    Update_command.attach(&getCommand,TIMER_UPDATE);

    t.start();
    while(1) {


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

}

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");
}