![](/media/cache/group/11700697_10200701668420946_8214886085951071495_o.jpg.50x50_q85.jpg)
mobile robot control with artificial hormones
Dependencies: Communication_Robot RTC-DS1307 iSerial mbed
Fork of Andante_v00 by
main.cpp
- Committer:
- soulx
- Date:
- 2017-06-07
- Revision:
- 9:d0454a04f433
- Parent:
- 8:af8fbf678f28
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); }