File content as of revision 9:d0454a04f433:

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

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

#include "Rtc_Ds1307.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


Rtc_Ds1307 rtc(RTC_SDA, RTC_SCL);
Rtc_Ds1307::Time_rtc tm = {};

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,0.1);
    Update_command.attach(&getCommand,TIMER_UPDATE);
    //Samping.attach(&getSensor,TIMER_SAMPING);
}


int main()
{
    Timer t;
    uint8_t action_state=1;
    uint8_t first_time=0;
    myled=0;


    Init_servo();
    Init_foreground();

    ANDANTE_PROTOCOL_PACKET packet;

    // test send datapacket
    packet.robotId = 0xBB;
    packet.length = 5;
    packet.instructionErrorId = 0x07;
    packet.parameter[0] = 255;
    packet.parameter[1] = 255;
    packet.parameter[2] = 255;


    pan_a.sendCommunicatePacket(&packet);

    t.start();
    while(1) {

        //   uint8_t status = pan_a.receiveCommunicatePacket(&packet);

        // pan_a.sendCommunicatePacket(&packet);
        switch(action_state) {
            case 1:// forward max speec
                if(first_time == 0) {

                    setFrontAngle(0.0);
                    setRearAngle(0.0);
                    setSpeedControl(1.0);
                    t.reset();
                    first_time=1;
                } else if(t.read() >= 1.0) {
                    setFrontAngle(0.0);
                    setRearAngle(0.0);
                    setSpeedControl(0.0);

                    action_state =2;
                    first_time = 0;
                }
                break;
            case 2://backward max speed
            break;
        }


    }
}


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 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.0f) {
        us = (FRONT_CENTER-FRONT_MIN)*precent +FRONT_CENTER;
    } else if(precent >0.0f) {
        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.0f) {
        us = (RC_SPEED_CENTER-RC_SPEED_MIN)*precent +RC_SPEED_CENTER;
    } else if(precent >0.0f) {
        us = (RC_SPEED_MAX-RC_SPEED_CENTER)*precent +RC_SPEED_CENTER;
    }
    drive_motor.pulsewidth_us(us);
}