File content as of revision 4:42dc950d54f4:

#include "main.h"
#include "mbed.h"
#include "param.h"
#include "pin_config.h"

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


//control stearing
PwmOut heading_front(PIN_FRONT_SERVO);
PwmOut heading_rear(PIN_REAR_SERVO);
//control speed
PwmOut drive_motor(PIN_RC_SPEED);

//led debugging
DigitalOut myled(LED1);

//set foreground
Ticker Update_command;
Ticker Samping;


//Init communication serial 2chanal
COMMUNICATION pan_a(TX_PAN_A, RX_PAN_A,115200,1000,1000); // tx, rx
COMMUNICATION pan_b(TX_PAN_B, RX_PAN_B,115200,1000,100); // tx, rx


void Init_servo()
{
    heading_front.period_ms(20);
    heading_rear.period_ms(20);
    drive_motor.period_ms(20);
}

void Init_foreground()
{
    Update_command.attach(&getCommand,TIMER_UPDATE);
    Samping.attach(&getSensor,TIMER_SAMPING);
}


int main()
{
    myled=0;


    Init_servo();

    ANDANTE_PROTOCOL_PACKET packet;

    packet.robotId = 0xBB;
    packet.length = 8;
    packet.instructionErrorId = 0x01;
    packet.parameter[0] = 0x02;
    packet.parameter[1] = 0x03;
    packet.parameter[2] = 0x04;
    packet.parameter[3] = 0x05;
    packet.parameter[4] = 0x06;
    packet.parameter[5] = 0x07;

    pan_a.sendCommunicatePacket(&packet);




}


void getCommand()
{
    static uint8_t count =0;

    ANDANTE_PROTOCOL_PACKET packet;

    uint8_t status = pan_a.receiveCommunicatePacket(&packet);

    if(status == ANDANTE_ERRBIT_NONE) {
        if(count >1) {
            count--;
        } else {
            count=0;
        }
        //update command
        setControl(&packet);

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

    }

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

void getSensor()
{

}

void setControl(ANDANTE_PROTOCOL_PACKET *packet)
{
    if(packet->instructionErrorId && ANDANTE_SPEED_CONTROL == ANDANTE_SPEED_CONTROL) {
        setSpeedControl(((int16_t)packet->parameter[0]-127)/128.0);
    }

    if(packet->instructionErrorId && ANDANTE_FRONT_ANGLE == ANDANTE_FRONT_ANGLE) {
        setFrontAngle(((int16_t)packet->parameter[1]-127)/128.0);
    }
    if(packet->instructionErrorId && ANDANTE_REAR_ANGLE == ANDANTE_REAR_ANGLE) {
        setRearAngle(((int16_t)packet->parameter[2]-127)/128.0);
    }
}


void setFrontAngle(float precent)
{
    int us=FRONT_CENTER;

    if(precent > (float)1.00 || precent < (float)-1.00) {
        heading_front.pulsewidth_us(us); //STOP
        return;
    }

    if(precent <0) {
        us = (FRONT_CENTER-FRONT_MIN)*precent +FRONT_CENTER;
    } else if(precent >0) {
        us = (FRONT_MAX-FRONT_CENTER)*precent +FRONT_CENTER;
    }
    heading_front.pulsewidth_us(us);
}

void setRearAngle(float precent)
{
    int us=REAR_CENTER;

    if(precent > (float)1.0 || precent < (float)-1.0) {
        heading_rear.pulsewidth_us(us); //STOP
        return;
    }

    if(precent <0) {
        us = (REAR_CENTER-REAR_MIN)*precent +REAR_CENTER;
    } else if(precent >0) {
        us = (REAR_MAX-REAR_CENTER)*precent +REAR_CENTER;
    }
    heading_rear.pulsewidth_us(us);
}

void setSpeedControl(float precent)
{
    int us=RC_SPEED_CENTER;

    if(precent > (float)1.0 || precent < (float)-1.0) {
        drive_motor.pulsewidth_us(us); //STOP
        return;
    }

    if(precent <0) {
        us = (RC_SPEED_CENTER-RC_SPEED_MIN)*precent +RC_SPEED_CENTER;
    } else if(precent >0) {
        us = (RC_SPEED_MAX-RC_SPEED_CENTER)*precent +RC_SPEED_CENTER;
    }
    drive_motor.pulsewidth_us(us);
}