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@8:af8fbf678f28, 2016-12-06 (annotated)

Committer:
soulx
Date:
Tue Dec 06 12:17:09 2016 +0000
Revision:
8:af8fbf678f28
Parent:
5:aae71a94cb41 
add test fix control

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 5:aae71a94cb41 10 #include "Rtc_Ds1307.h"
soulx 5:aae71a94cb41 11
soulx 2:b31c4677ebc0 12
soulx 0:95d17607a698 13 //control stearing
soulx 0:95d17607a698 14 PwmOut heading_front(PIN_FRONT_SERVO);
soulx 0:95d17607a698 15 PwmOut heading_rear(PIN_REAR_SERVO);
soulx 0:95d17607a698 16 //control speed
soulx 0:95d17607a698 17 PwmOut drive_motor(PIN_RC_SPEED);
soulx 0:95d17607a698 18
soulx 0:95d17607a698 19 //led debugging
soulx 0:95d17607a698 20 DigitalOut myled(LED1);
soulx 0:95d17607a698 21
soulx 0:95d17607a698 22 //set foreground
soulx 0:95d17607a698 23 Ticker Update_command;
soulx 0:95d17607a698 24 Ticker Samping;
soulx 0:95d17607a698 25
soulx 4:42dc950d54f4 26
soulx 4:42dc950d54f4 27 //Init communication serial 2chanal
soulx 4:42dc950d54f4 28 COMMUNICATION pan_a(TX_PAN_A, RX_PAN_A,115200,1000,1000); // tx, rx
soulx 4:42dc950d54f4 29 COMMUNICATION pan_b(TX_PAN_B, RX_PAN_B,115200,1000,100); // tx, rx
soulx 4:42dc950d54f4 30
soulx 4:42dc950d54f4 31
soulx 5:aae71a94cb41 32 Rtc_Ds1307 rtc(RTC_SDA, RTC_SCL);
soulx 5:aae71a94cb41 33 Rtc_Ds1307::Time_rtc tm = {};
soulx 5:aae71a94cb41 34
soulx 0:95d17607a698 35 void Init_servo()
soulx 0:95d17607a698 36 {
soulx 0:95d17607a698 37 heading_front.period_ms(20);
soulx 0:95d17607a698 38 heading_rear.period_ms(20);
soulx 0:95d17607a698 39 drive_motor.period_ms(20);
soulx 0:95d17607a698 40 }
soulx 0:95d17607a698 41
soulx 4:42dc950d54f4 42 void Init_foreground()
soulx 4:42dc950d54f4 43 {
soulx 5:aae71a94cb41 44 //Update_command.attach(&getCommand,0.1);
soulx 4:42dc950d54f4 45 Update_command.attach(&getCommand,TIMER_UPDATE);
soulx 5:aae71a94cb41 46 //Samping.attach(&getSensor,TIMER_SAMPING);
soulx 4:42dc950d54f4 47 }
soulx 4:42dc950d54f4 48
soulx 4:42dc950d54f4 49
soulx 4:42dc950d54f4 50 int main()
soulx 2:b31c4677ebc0 51 {
soulx 8:af8fbf678f28 52 Timer t;
soulx 8:af8fbf678f28 53 uint8_t action_state=1;
soulx 8:af8fbf678f28 54 uint8_t first_time=0;
soulx 4:42dc950d54f4 55 myled=0;
soulx 2:b31c4677ebc0 56
soulx 4:42dc950d54f4 57
soulx 4:42dc950d54f4 58 Init_servo();
soulx 5:aae71a94cb41 59 Init_foreground();
soulx 4:42dc950d54f4 60
soulx 4:42dc950d54f4 61 ANDANTE_PROTOCOL_PACKET packet;
soulx 4:42dc950d54f4 62
soulx 5:aae71a94cb41 63 // test send datapacket
soulx 4:42dc950d54f4 64 packet.robotId = 0xBB;
soulx 5:aae71a94cb41 65 packet.length = 5;
soulx 5:aae71a94cb41 66 packet.instructionErrorId = 0x07;
soulx 5:aae71a94cb41 67 packet.parameter[0] = 255;
soulx 5:aae71a94cb41 68 packet.parameter[1] = 255;
soulx 5:aae71a94cb41 69 packet.parameter[2] = 255;
soulx 5:aae71a94cb41 70
soulx 4:42dc950d54f4 71
soulx 4:42dc950d54f4 72 pan_a.sendCommunicatePacket(&packet);
soulx 8:af8fbf678f28 73
soulx 8:af8fbf678f28 74 t.start();
soulx 5:aae71a94cb41 75 while(1) {
soulx 4:42dc950d54f4 76
soulx 5:aae71a94cb41 77 // uint8_t status = pan_a.receiveCommunicatePacket(&packet);
soulx 4:42dc950d54f4 78
soulx 5:aae71a94cb41 79 // pan_a.sendCommunicatePacket(&packet);
soulx 8:af8fbf678f28 80 switch(action_state) {
soulx 8:af8fbf678f28 81 case 1:// forward max speec
soulx 8:af8fbf678f28 82 if(first_time == 0) {
soulx 8:af8fbf678f28 83
soulx 8:af8fbf678f28 84 setFrontAngle(0.0);
soulx 8:af8fbf678f28 85 setRearAngle(0.0);
soulx 8:af8fbf678f28 86 setSpeedControl(1.0);
soulx 8:af8fbf678f28 87 t.reset();
soulx 8:af8fbf678f28 88 first_time=1;
soulx 8:af8fbf678f28 89 } else if(t.read() >= 1.0) {
soulx 8:af8fbf678f28 90 setFrontAngle(0.0);
soulx 8:af8fbf678f28 91 setRearAngle(0.0);
soulx 8:af8fbf678f28 92 setSpeedControl(0.0);
soulx 8:af8fbf678f28 93
soulx 8:af8fbf678f28 94 action_state =2;
soulx 8:af8fbf678f28 95 first_time = 0;
soulx 8:af8fbf678f28 96 }
soulx 8:af8fbf678f28 97 break;
soulx 8:af8fbf678f28 98 case 2://backward max speed
soulx 8:af8fbf678f28 99 break;
soulx 8:af8fbf678f28 100 }
soulx 8:af8fbf678f28 101
soulx 8:af8fbf678f28 102
soulx 5:aae71a94cb41 103 }
soulx 4:42dc950d54f4 104 }
soulx 4:42dc950d54f4 105
soulx 4:42dc950d54f4 106
soulx 4:42dc950d54f4 107 void getCommand()
soulx 4:42dc950d54f4 108 {
soulx 4:42dc950d54f4 109 static uint8_t count =0;
soulx 4:42dc950d54f4 110
soulx 4:42dc950d54f4 111 ANDANTE_PROTOCOL_PACKET packet;
soulx 4:42dc950d54f4 112
soulx 4:42dc950d54f4 113 uint8_t status = pan_a.receiveCommunicatePacket(&packet);
soulx 5:aae71a94cb41 114 myled=0;
soulx 8:af8fbf678f28 115
soulx 5:aae71a94cb41 116
soulx 4:42dc950d54f4 117
soulx 4:42dc950d54f4 118 if(status == ANDANTE_ERRBIT_NONE) {
soulx 5:aae71a94cb41 119 if(count >2 && count <10) {
soulx 4:42dc950d54f4 120 count--;
soulx 4:42dc950d54f4 121 } else {
soulx 4:42dc950d54f4 122 count=0;
soulx 8:af8fbf678f28 123
soulx 4:42dc950d54f4 124 }
soulx 5:aae71a94cb41 125 pan_a.sendCommunicatePacket(&packet);
soulx 4:42dc950d54f4 126 //update command
soulx 4:42dc950d54f4 127 setControl(&packet);
soulx 4:42dc950d54f4 128
soulx 4:42dc950d54f4 129 } else if(status == ANDANTE_ERRBIT_READ_TIMEOUT) {
soulx 4:42dc950d54f4 130 count++;
soulx 4:42dc950d54f4 131 }
soulx 4:42dc950d54f4 132
soulx 4:42dc950d54f4 133 if(count >= TIMEOUT_RESPONE_COMMAND) {
soulx 4:42dc950d54f4 134 //stop robot
soulx 5:aae71a94cb41 135 count++;
soulx 5:aae71a94cb41 136 myled=1;
soulx 4:42dc950d54f4 137 setSpeedControl(0.0);
soulx 4:42dc950d54f4 138 }
soulx 2:b31c4677ebc0 139 }
soulx 2:b31c4677ebc0 140
soulx 2:b31c4677ebc0 141 void getSensor()
soulx 2:b31c4677ebc0 142 {
soulx 0:95d17607a698 143
soulx 0:95d17607a698 144 }
soulx 0:95d17607a698 145
soulx 4:42dc950d54f4 146 void setControl(ANDANTE_PROTOCOL_PACKET *packet)
soulx 0:95d17607a698 147 {
soulx 5:aae71a94cb41 148
soulx 4:42dc950d54f4 149 if(packet->instructionErrorId && ANDANTE_SPEED_CONTROL == ANDANTE_SPEED_CONTROL) {
soulx 4:42dc950d54f4 150 setSpeedControl(((int16_t)packet->parameter[0]-127)/128.0);
soulx 4:42dc950d54f4 151 }
soulx 4:42dc950d54f4 152
soulx 4:42dc950d54f4 153 if(packet->instructionErrorId && ANDANTE_FRONT_ANGLE == ANDANTE_FRONT_ANGLE) {
soulx 4:42dc950d54f4 154 setFrontAngle(((int16_t)packet->parameter[1]-127)/128.0);
soulx 4:42dc950d54f4 155 }
soulx 4:42dc950d54f4 156 if(packet->instructionErrorId && ANDANTE_REAR_ANGLE == ANDANTE_REAR_ANGLE) {
soulx 4:42dc950d54f4 157 setRearAngle(((int16_t)packet->parameter[2]-127)/128.0);
soulx 4:42dc950d54f4 158 }
soulx 0:95d17607a698 159 }
soulx 0:95d17607a698 160
soulx 0:95d17607a698 161
soulx 4:42dc950d54f4 162 void setFrontAngle(float precent)
soulx 0:95d17607a698 163 {
soulx 4:42dc950d54f4 164 int us=FRONT_CENTER;
soulx 4:42dc950d54f4 165
soulx 4:42dc950d54f4 166 if(precent > (float)1.00 || precent < (float)-1.00) {
soulx 4:42dc950d54f4 167 heading_front.pulsewidth_us(us); //STOP
soulx 4:42dc950d54f4 168 return;
soulx 4:42dc950d54f4 169 }
soulx 3:12f2ee26a43f 170
soulx 5:aae71a94cb41 171 if(precent <0.0f) {
soulx 4:42dc950d54f4 172 us = (FRONT_CENTER-FRONT_MIN)*precent +FRONT_CENTER;
soulx 5:aae71a94cb41 173 } else if(precent >0.0f) {
soulx 4:42dc950d54f4 174 us = (FRONT_MAX-FRONT_CENTER)*precent +FRONT_CENTER;
soulx 4:42dc950d54f4 175 }
soulx 4:42dc950d54f4 176 heading_front.pulsewidth_us(us);
soulx 4:42dc950d54f4 177 }
soulx 4:42dc950d54f4 178
soulx 4:42dc950d54f4 179 void setRearAngle(float precent)
soulx 4:42dc950d54f4 180 {
soulx 4:42dc950d54f4 181 int us=REAR_CENTER;
soulx 0:95d17607a698 182
soulx 4:42dc950d54f4 183 if(precent > (float)1.0 || precent < (float)-1.0) {
soulx 4:42dc950d54f4 184 heading_rear.pulsewidth_us(us); //STOP
soulx 4:42dc950d54f4 185 return;
soulx 4:42dc950d54f4 186 }
soulx 4:42dc950d54f4 187
soulx 4:42dc950d54f4 188 if(precent <0) {
soulx 4:42dc950d54f4 189 us = (REAR_CENTER-REAR_MIN)*precent +REAR_CENTER;
soulx 4:42dc950d54f4 190 } else if(precent >0) {
soulx 4:42dc950d54f4 191 us = (REAR_MAX-REAR_CENTER)*precent +REAR_CENTER;
soulx 4:42dc950d54f4 192 }
soulx 4:42dc950d54f4 193 heading_rear.pulsewidth_us(us);
soulx 4:42dc950d54f4 194 }
soulx 3:12f2ee26a43f 195
soulx 4:42dc950d54f4 196 void setSpeedControl(float precent)
soulx 4:42dc950d54f4 197 {
soulx 4:42dc950d54f4 198 int us=RC_SPEED_CENTER;
soulx 3:12f2ee26a43f 199
soulx 4:42dc950d54f4 200 if(precent > (float)1.0 || precent < (float)-1.0) {
soulx 4:42dc950d54f4 201 drive_motor.pulsewidth_us(us); //STOP
soulx 4:42dc950d54f4 202 return;
soulx 4:42dc950d54f4 203 }
soulx 3:12f2ee26a43f 204
soulx 5:aae71a94cb41 205 if(precent <0.0f) {
soulx 4:42dc950d54f4 206 us = (RC_SPEED_CENTER-RC_SPEED_MIN)*precent +RC_SPEED_CENTER;
soulx 5:aae71a94cb41 207 } else if(precent >0.0f) {
soulx 4:42dc950d54f4 208 us = (RC_SPEED_MAX-RC_SPEED_CENTER)*precent +RC_SPEED_CENTER;
soulx 3:12f2ee26a43f 209 }
soulx 4:42dc950d54f4 210 drive_motor.pulsewidth_us(us);
soulx 4:42dc950d54f4 211 }