BE@R lab / Mbed 2 deprecated Andante_v000

mobile robot control with artificial hormones

Dependencies:   Communication_Robot RTC-DS1307 iSerial mbed

Fork of Andante_v00 by potiwat ngamkajornwiwat

main.cpp@4:42dc950d54f4, 2015-03-02 (annotated)

Committer:
soulx
Date:
Mon Mar 02 05:53:20 2015 +0000
Revision:
4:42dc950d54f4
Parent:
3:12f2ee26a43f
Child:
5:aae71a94cb41
add function control robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soulx 4:42dc950d54f4 1 #include "main.h"
soulx 0:95d17607a698 2 #include "mbed.h"
soulx 0:95d17607a698 3 #include "param.h"
soulx 0:95d17607a698 4 #include "pin_config.h"
soulx 0:95d17607a698 5
soulx 4:42dc950d54f4 6 //#include "iSerial.h"
soulx 2:b31c4677ebc0 7 #include "communication.h"
soulx 4:42dc950d54f4 8 #include "protocol.h"
soulx 2:b31c4677ebc0 9
soulx 2:b31c4677ebc0 10
soulx 0:95d17607a698 11 //control stearing
soulx 0:95d17607a698 12 PwmOut heading_front(PIN_FRONT_SERVO);
soulx 0:95d17607a698 13 PwmOut heading_rear(PIN_REAR_SERVO);
soulx 0:95d17607a698 14 //control speed
soulx 0:95d17607a698 15 PwmOut drive_motor(PIN_RC_SPEED);
soulx 0:95d17607a698 16
soulx 0:95d17607a698 17 //led debugging
soulx 0:95d17607a698 18 DigitalOut myled(LED1);
soulx 0:95d17607a698 19
soulx 0:95d17607a698 20 //set foreground
soulx 0:95d17607a698 21 Ticker Update_command;
soulx 0:95d17607a698 22 Ticker Samping;
soulx 0:95d17607a698 23
soulx 4:42dc950d54f4 24
soulx 4:42dc950d54f4 25 //Init communication serial 2chanal
soulx 4:42dc950d54f4 26 COMMUNICATION pan_a(TX_PAN_A, RX_PAN_A,115200,1000,1000); // tx, rx
soulx 4:42dc950d54f4 27 COMMUNICATION pan_b(TX_PAN_B, RX_PAN_B,115200,1000,100); // tx, rx
soulx 4:42dc950d54f4 28
soulx 4:42dc950d54f4 29
soulx 0:95d17607a698 30 void Init_servo()
soulx 0:95d17607a698 31 {
soulx 0:95d17607a698 32 heading_front.period_ms(20);
soulx 0:95d17607a698 33 heading_rear.period_ms(20);
soulx 0:95d17607a698 34 drive_motor.period_ms(20);
soulx 0:95d17607a698 35 }
soulx 0:95d17607a698 36
soulx 4:42dc950d54f4 37 void Init_foreground()
soulx 4:42dc950d54f4 38 {
soulx 4:42dc950d54f4 39 Update_command.attach(&getCommand,TIMER_UPDATE);
soulx 4:42dc950d54f4 40 Samping.attach(&getSensor,TIMER_SAMPING);
soulx 4:42dc950d54f4 41 }
soulx 4:42dc950d54f4 42
soulx 4:42dc950d54f4 43
soulx 4:42dc950d54f4 44 int main()
soulx 2:b31c4677ebc0 45 {
soulx 4:42dc950d54f4 46 myled=0;
soulx 2:b31c4677ebc0 47
soulx 4:42dc950d54f4 48
soulx 4:42dc950d54f4 49 Init_servo();
soulx 4:42dc950d54f4 50
soulx 4:42dc950d54f4 51 ANDANTE_PROTOCOL_PACKET packet;
soulx 4:42dc950d54f4 52
soulx 4:42dc950d54f4 53 packet.robotId = 0xBB;
soulx 4:42dc950d54f4 54 packet.length = 8;
soulx 4:42dc950d54f4 55 packet.instructionErrorId = 0x01;
soulx 4:42dc950d54f4 56 packet.parameter[0] = 0x02;
soulx 4:42dc950d54f4 57 packet.parameter[1] = 0x03;
soulx 4:42dc950d54f4 58 packet.parameter[2] = 0x04;
soulx 4:42dc950d54f4 59 packet.parameter[3] = 0x05;
soulx 4:42dc950d54f4 60 packet.parameter[4] = 0x06;
soulx 4:42dc950d54f4 61 packet.parameter[5] = 0x07;
soulx 4:42dc950d54f4 62
soulx 4:42dc950d54f4 63 pan_a.sendCommunicatePacket(&packet);
soulx 4:42dc950d54f4 64
soulx 4:42dc950d54f4 65
soulx 4:42dc950d54f4 66
soulx 4:42dc950d54f4 67
soulx 4:42dc950d54f4 68 }
soulx 4:42dc950d54f4 69
soulx 4:42dc950d54f4 70
soulx 4:42dc950d54f4 71 void getCommand()
soulx 4:42dc950d54f4 72 {
soulx 4:42dc950d54f4 73 static uint8_t count =0;
soulx 4:42dc950d54f4 74
soulx 4:42dc950d54f4 75 ANDANTE_PROTOCOL_PACKET packet;
soulx 4:42dc950d54f4 76
soulx 4:42dc950d54f4 77 uint8_t status = pan_a.receiveCommunicatePacket(&packet);
soulx 4:42dc950d54f4 78
soulx 4:42dc950d54f4 79 if(status == ANDANTE_ERRBIT_NONE) {
soulx 4:42dc950d54f4 80 if(count >1) {
soulx 4:42dc950d54f4 81 count--;
soulx 4:42dc950d54f4 82 } else {
soulx 4:42dc950d54f4 83 count=0;
soulx 4:42dc950d54f4 84 }
soulx 4:42dc950d54f4 85 //update command
soulx 4:42dc950d54f4 86 setControl(&packet);
soulx 4:42dc950d54f4 87
soulx 4:42dc950d54f4 88 } else if(status == ANDANTE_ERRBIT_READ_TIMEOUT) {
soulx 4:42dc950d54f4 89 count++;
soulx 4:42dc950d54f4 90
soulx 4:42dc950d54f4 91 }
soulx 4:42dc950d54f4 92
soulx 4:42dc950d54f4 93 if(count >= TIMEOUT_RESPONE_COMMAND) {
soulx 4:42dc950d54f4 94 //stop robot
soulx 4:42dc950d54f4 95 setSpeedControl(0.0);
soulx 4:42dc950d54f4 96 }
soulx 2:b31c4677ebc0 97 }
soulx 2:b31c4677ebc0 98
soulx 2:b31c4677ebc0 99 void getSensor()
soulx 2:b31c4677ebc0 100 {
soulx 0:95d17607a698 101
soulx 0:95d17607a698 102 }
soulx 0:95d17607a698 103
soulx 4:42dc950d54f4 104 void setControl(ANDANTE_PROTOCOL_PACKET *packet)
soulx 0:95d17607a698 105 {
soulx 4:42dc950d54f4 106 if(packet->instructionErrorId && ANDANTE_SPEED_CONTROL == ANDANTE_SPEED_CONTROL) {
soulx 4:42dc950d54f4 107 setSpeedControl(((int16_t)packet->parameter[0]-127)/128.0);
soulx 4:42dc950d54f4 108 }
soulx 4:42dc950d54f4 109
soulx 4:42dc950d54f4 110 if(packet->instructionErrorId && ANDANTE_FRONT_ANGLE == ANDANTE_FRONT_ANGLE) {
soulx 4:42dc950d54f4 111 setFrontAngle(((int16_t)packet->parameter[1]-127)/128.0);
soulx 4:42dc950d54f4 112 }
soulx 4:42dc950d54f4 113 if(packet->instructionErrorId && ANDANTE_REAR_ANGLE == ANDANTE_REAR_ANGLE) {
soulx 4:42dc950d54f4 114 setRearAngle(((int16_t)packet->parameter[2]-127)/128.0);
soulx 4:42dc950d54f4 115 }
soulx 0:95d17607a698 116 }
soulx 0:95d17607a698 117
soulx 0:95d17607a698 118
soulx 4:42dc950d54f4 119 void setFrontAngle(float precent)
soulx 0:95d17607a698 120 {
soulx 4:42dc950d54f4 121 int us=FRONT_CENTER;
soulx 4:42dc950d54f4 122
soulx 4:42dc950d54f4 123 if(precent > (float)1.00 || precent < (float)-1.00) {
soulx 4:42dc950d54f4 124 heading_front.pulsewidth_us(us); //STOP
soulx 4:42dc950d54f4 125 return;
soulx 4:42dc950d54f4 126 }
soulx 3:12f2ee26a43f 127
soulx 4:42dc950d54f4 128 if(precent <0) {
soulx 4:42dc950d54f4 129 us = (FRONT_CENTER-FRONT_MIN)*precent +FRONT_CENTER;
soulx 4:42dc950d54f4 130 } else if(precent >0) {
soulx 4:42dc950d54f4 131 us = (FRONT_MAX-FRONT_CENTER)*precent +FRONT_CENTER;
soulx 4:42dc950d54f4 132 }
soulx 4:42dc950d54f4 133 heading_front.pulsewidth_us(us);
soulx 4:42dc950d54f4 134 }
soulx 4:42dc950d54f4 135
soulx 4:42dc950d54f4 136 void setRearAngle(float precent)
soulx 4:42dc950d54f4 137 {
soulx 4:42dc950d54f4 138 int us=REAR_CENTER;
soulx 0:95d17607a698 139
soulx 4:42dc950d54f4 140 if(precent > (float)1.0 || precent < (float)-1.0) {
soulx 4:42dc950d54f4 141 heading_rear.pulsewidth_us(us); //STOP
soulx 4:42dc950d54f4 142 return;
soulx 4:42dc950d54f4 143 }
soulx 4:42dc950d54f4 144
soulx 4:42dc950d54f4 145 if(precent <0) {
soulx 4:42dc950d54f4 146 us = (REAR_CENTER-REAR_MIN)*precent +REAR_CENTER;
soulx 4:42dc950d54f4 147 } else if(precent >0) {
soulx 4:42dc950d54f4 148 us = (REAR_MAX-REAR_CENTER)*precent +REAR_CENTER;
soulx 4:42dc950d54f4 149 }
soulx 4:42dc950d54f4 150 heading_rear.pulsewidth_us(us);
soulx 4:42dc950d54f4 151 }
soulx 3:12f2ee26a43f 152
soulx 4:42dc950d54f4 153 void setSpeedControl(float precent)
soulx 4:42dc950d54f4 154 {
soulx 4:42dc950d54f4 155 int us=RC_SPEED_CENTER;
soulx 3:12f2ee26a43f 156
soulx 4:42dc950d54f4 157 if(precent > (float)1.0 || precent < (float)-1.0) {
soulx 4:42dc950d54f4 158 drive_motor.pulsewidth_us(us); //STOP
soulx 4:42dc950d54f4 159 return;
soulx 4:42dc950d54f4 160 }
soulx 3:12f2ee26a43f 161
soulx 4:42dc950d54f4 162 if(precent <0) {
soulx 4:42dc950d54f4 163 us = (RC_SPEED_CENTER-RC_SPEED_MIN)*precent +RC_SPEED_CENTER;
soulx 4:42dc950d54f4 164 } else if(precent >0) {
soulx 4:42dc950d54f4 165 us = (RC_SPEED_MAX-RC_SPEED_CENTER)*precent +RC_SPEED_CENTER;
soulx 3:12f2ee26a43f 166 }
soulx 4:42dc950d54f4 167 drive_motor.pulsewidth_us(us);
soulx 4:42dc950d54f4 168 }