BE@R lab / Mbed 2 deprecated dog_V3_3_testmotor

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Communication_Robot QEI iSerial mbed motion_control

Fork of dog_V3_2_testmotor by BE@R lab

main.cpp

Committer:
soulx
Date:
2015-07-20
Revision:
12:2564eac22e0a
Parent:
11:336dd293daa1
Child:
13:218c22a620cc

File content as of revision 12:2564eac22e0a:

#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 test_status();
void test_limit();
void test_motor();

void routine();

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;

//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(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);

    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;
    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;
            }
        }
    }
    //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");
    while(1) {
        pc.printf("leftUP_limit_up = ");
        if(leg_left_upper.GetLimitUp() == 0)
            pc.printf("shot\n");
        else
            pc.printf("open\n");

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

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

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

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

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

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

        pc.printf("rightLow_limit_down = ");
        if(leg_right_lower.GetLimitDown() == 0)
            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);
            }
        }
    }
}