Fer Pena / Mbed 2 deprecated RobotV1

aaa

Dependencies:   mbed

Fork of RobotV1 by Gerardo Carmona

main.cpp@6:9e9143e97bae, 2016-04-14 (annotated)

Committer:
fdopc
Date:
Thu Apr 14 17:17:56 2016 +0000
Revision:
6:9e9143e97bae
Parent:
3:07a21d1d16e4 
Bluetooth Rx For RC car

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gcarmonar 0:4a46db9b1f3e 1 #include "mbed.h"
gcarmonar 0:4a46db9b1f3e 2
fdopc 6:9e9143e97bae 3 #define ERROR 0
fdopc 6:9e9143e97bae 4 #define VALID 1
fdopc 6:9e9143e97bae 5
fdopc 6:9e9143e97bae 6 DigitalOut R(LED_RED);
fdopc 6:9e9143e97bae 7 Ticker TimeoutTimer;
fdopc 6:9e9143e97bae 8 Serial pc(USBTX, USBRX);
fdopc 6:9e9143e97bae 9 Serial bt(PTE0, PTE1);
fdopc 6:9e9143e97bae 10 DigitalOut motorFront_1(D11); //pines de sentido del motor delantero y trasero
fdopc 6:9e9143e97bae 11 DigitalOut motorFront_2(D12);
fdopc 6:9e9143e97bae 12
fdopc 6:9e9143e97bae 13 DigitalOut motorTurn_1(D9); //pines de sentido del motor volante
fdopc 6:9e9143e97bae 14 DigitalOut motorTurn_2(D8);
fdopc 6:9e9143e97bae 15
fdopc 6:9e9143e97bae 16 PwmOut pwm_motorUPDOWN(D7);
fdopc 6:9e9143e97bae 17 PwmOut pwm_motorTURN(D5);
fdopc 6:9e9143e97bae 18
fdopc 6:9e9143e97bae 19 void SetMOTOR(int *, int *, float *, float *,int *, int *,int *, int *);
fdopc 6:9e9143e97bae 20 void go_forward(void);
fdopc 6:9e9143e97bae 21 void go_reverse(void);
fdopc 6:9e9143e97bae 22 void stop(void);
fdopc 6:9e9143e97bae 23 void turn_left(void);
fdopc 6:9e9143e97bae 24 void turn_right(void);
fdopc 6:9e9143e97bae 25 void turn_none(void);
fdopc 6:9e9143e97bae 26 //Global variables
fdopc 6:9e9143e97bae 27 char c;
fdopc 6:9e9143e97bae 28
fdopc 6:9e9143e97bae 29 ////Functions/////
fdopc 6:9e9143e97bae 30 //Timeout
fdopc 6:9e9143e97bae 31 void TimeOutRoutine()
fdopc 6:9e9143e97bae 32 {
fdopc 6:9e9143e97bae 33 pwm_motorUPDOWN.write(0.0);
fdopc 6:9e9143e97bae 34 pwm_motorTURN.write(0.0);
fdopc 6:9e9143e97bae 35 R=0;
fdopc 6:9e9143e97bae 36 }
fdopc 6:9e9143e97bae 37
fdopc 6:9e9143e97bae 38 //Coversion to PWM vals
fdopc 6:9e9143e97bae 39 float min(float a, float b){
fdopc 6:9e9143e97bae 40 return (a>b)?b:a;
fdopc 6:9e9143e97bae 41 }
fdopc 6:9e9143e97bae 42 float ConvertSteer(uint8_t val)
fdopc 6:9e9143e97bae 43 {
fdopc 6:9e9143e97bae 44 float temp;
fdopc 6:9e9143e97bae 45 if(val>127)
fdopc 6:9e9143e97bae 46 temp=(val-127)/128.0;
fdopc 6:9e9143e97bae 47 else
fdopc 6:9e9143e97bae 48 temp=(127-val)/128.0;
fdopc 6:9e9143e97bae 49
fdopc 6:9e9143e97bae 50 return temp;
fdopc 6:9e9143e97bae 51 }
fdopc 6:9e9143e97bae 52
fdopc 6:9e9143e97bae 53 //Decode frame from Bluetooth
fdopc 6:9e9143e97bae 54 uint8_t DecodeBT(void)
fdopc 6:9e9143e97bae 55 {
fdopc 6:9e9143e97bae 56 float SteerPWMVal, PedalPWMVal;
fdopc 6:9e9143e97bae 57 uint8_t Steer=0,Pedal=0;
fdopc 6:9e9143e97bae 58 c = bt.getc();
fdopc 6:9e9143e97bae 59 if(c=='S') { //Check Start of frame
fdopc 6:9e9143e97bae 60 while(!bt.readable());
fdopc 6:9e9143e97bae 61 c = bt.getc();
fdopc 6:9e9143e97bae 62 if(c=='T') { //Check Start of frame
fdopc 6:9e9143e97bae 63 while(!bt.readable());
fdopc 6:9e9143e97bae 64 //Read and convert data
fdopc 6:9e9143e97bae 65 Steer = bt.getc();
fdopc 6:9e9143e97bae 66 while(!bt.readable());
fdopc 6:9e9143e97bae 67 Pedal = bt.getc();
fdopc 6:9e9143e97bae 68
fdopc 6:9e9143e97bae 69 if(Steer>146)
fdopc 6:9e9143e97bae 70 turn_right();
fdopc 6:9e9143e97bae 71 else if(Steer<110)
fdopc 6:9e9143e97bae 72 turn_left();
fdopc 6:9e9143e97bae 73 else
fdopc 6:9e9143e97bae 74 turn_none();
fdopc 6:9e9143e97bae 75 if(Pedal>146)
fdopc 6:9e9143e97bae 76 go_forward();
fdopc 6:9e9143e97bae 77 else if(Pedal<110)
fdopc 6:9e9143e97bae 78 go_reverse();
fdopc 6:9e9143e97bae 79 else
fdopc 6:9e9143e97bae 80 stop();
fdopc 6:9e9143e97bae 81 SteerPWMVal=min(ConvertSteer(Steer)*2.3,.9);
fdopc 6:9e9143e97bae 82 PedalPWMVal=min(ConvertSteer(Pedal)*2.3,1);
fdopc 6:9e9143e97bae 83
fdopc 6:9e9143e97bae 84 //*sw2=(SteerPWMVal>0);
fdopc 6:9e9143e97bae 85 //*sw1=(PedalPWMVal>0);
fdopc 6:9e9143e97bae 86
fdopc 6:9e9143e97bae 87
fdopc 6:9e9143e97bae 88 pwm_motorUPDOWN=PedalPWMVal;
fdopc 6:9e9143e97bae 89 pwm_motorTURN=SteerPWMVal;
fdopc 6:9e9143e97bae 90 PedalPWMVal=pwm_motorUPDOWN;
fdopc 6:9e9143e97bae 91 SteerPWMVal=pwm_motorTURN;
fdopc 6:9e9143e97bae 92 pc.printf("Steer : %f\t SW1=%i \t ", SteerPWMVal, Steer);
fdopc 6:9e9143e97bae 93 pc.printf("Pedal : %f\t SW2=%i\n\r", PedalPWMVal, Pedal);
fdopc 6:9e9143e97bae 94 R=1;
fdopc 6:9e9143e97bae 95 return VALID; //Valid frame received
fdopc 6:9e9143e97bae 96 }
fdopc 6:9e9143e97bae 97 }
fdopc 6:9e9143e97bae 98 return ERROR;
fdopc 6:9e9143e97bae 99 }
fdopc 6:9e9143e97bae 100
fdopc 6:9e9143e97bae 101
fdopc 6:9e9143e97bae 102 int main()
fdopc 6:9e9143e97bae 103 {
fdopc 6:9e9143e97bae 104 int Currnt_st_UPDOWN = 0;
fdopc 6:9e9143e97bae 105 int Befr_st_UPDOWN = 1;
fdopc 6:9e9143e97bae 106 int Currnt_st_LEFT_RIGTH = 0;
fdopc 6:9e9143e97bae 107 int Befr_st_LEFT_RIGTH = 1;
fdopc 6:9e9143e97bae 108 int mySW_UPDOWN =0;
fdopc 6:9e9143e97bae 109 int mySW_LEFT_RIGTH = 0;
fdopc 6:9e9143e97bae 110
fdopc 6:9e9143e97bae 111 float PWM_UPDOWN = 0.0;
fdopc 6:9e9143e97bae 112 float PWM_LEFT_RIGTH = 0.0;
fdopc 6:9e9143e97bae 113 R=1;
fdopc 6:9e9143e97bae 114 pwm_motorUPDOWN.period_us(50); //10 kHz
fdopc 6:9e9143e97bae 115 pwm_motorTURN.period_us(50); //10 kHz
fdopc 6:9e9143e97bae 116 pwm_motorUPDOWN.write(PWM_UPDOWN);
fdopc 6:9e9143e97bae 117 pwm_motorTURN.write(PWM_LEFT_RIGTH);
fdopc 6:9e9143e97bae 118
fdopc 6:9e9143e97bae 119
gcarmonar 0:4a46db9b1f3e 120
fdopc 3:07a21d1d16e4 121
fdopc 3:07a21d1d16e4 122 bt.baud(9600);
gcarmonar 0:4a46db9b1f3e 123 while(1) {
fdopc 3:07a21d1d16e4 124 while(!bt.readable());
fdopc 6:9e9143e97bae 125 if(DecodeBT()==VALID) {
fdopc 6:9e9143e97bae 126 //SetMOTOR(&mySW_UPDOWN, &mySW_LEFT_RIGTH, &PWM_UPDOWN, &PWM_LEFT_RIGTH, &Currnt_st_UPDOWN, &Befr_st_UPDOWN, &Currnt_st_LEFT_RIGTH, &Befr_st_LEFT_RIGTH);
fdopc 6:9e9143e97bae 127 TimeoutTimer.detach();
fdopc 6:9e9143e97bae 128 TimeoutTimer.attach_us(&TimeOutRoutine,200000);
fdopc 6:9e9143e97bae 129 }
fdopc 6:9e9143e97bae 130 }
fdopc 6:9e9143e97bae 131
fdopc 6:9e9143e97bae 132 }
fdopc 6:9e9143e97bae 133 void SetMOTOR(int *ptr_mySW_UPDOWN, int *ptr_mySW_LEFT_RIGTH, float *ptr_PWM_UPDOWN, float *ptr_PWM_LEFT_RIGTH,int *ptr_Currnt_st_UPDOWN, int *ptr_Befr_st_UPDOWN,int *ptr_Currnt_st_LEFT_RIGTH, int *ptr_Befr_st_LEFT_RIGTH)
fdopc 6:9e9143e97bae 134 {
fdopc 6:9e9143e97bae 135
fdopc 3:07a21d1d16e4 136
fdopc 6:9e9143e97bae 137 if(*ptr_mySW_UPDOWN==1) {
fdopc 6:9e9143e97bae 138 go_forward();
fdopc 6:9e9143e97bae 139 *ptr_Currnt_st_UPDOWN = 1;
fdopc 6:9e9143e97bae 140 if(*ptr_Currnt_st_UPDOWN!=*ptr_Befr_st_UPDOWN) {
fdopc 6:9e9143e97bae 141 pwm_motorUPDOWN.write(*ptr_PWM_UPDOWN);
fdopc 6:9e9143e97bae 142 *ptr_Befr_st_UPDOWN = *ptr_Currnt_st_UPDOWN;
fdopc 6:9e9143e97bae 143 }
fdopc 6:9e9143e97bae 144
fdopc 6:9e9143e97bae 145 } else {
fdopc 6:9e9143e97bae 146 go_reverse();
fdopc 6:9e9143e97bae 147 *ptr_Currnt_st_UPDOWN = 0;
fdopc 6:9e9143e97bae 148 if(*ptr_Currnt_st_UPDOWN!=*ptr_Befr_st_UPDOWN) {
fdopc 6:9e9143e97bae 149 pwm_motorUPDOWN.write(*ptr_PWM_UPDOWN);
fdopc 6:9e9143e97bae 150 *ptr_Befr_st_UPDOWN = *ptr_Currnt_st_UPDOWN;
fdopc 6:9e9143e97bae 151 }
fdopc 6:9e9143e97bae 152
fdopc 6:9e9143e97bae 153 }
fdopc 6:9e9143e97bae 154 if(*ptr_mySW_LEFT_RIGTH==1) {
fdopc 6:9e9143e97bae 155 turn_left();
fdopc 6:9e9143e97bae 156 *ptr_Currnt_st_LEFT_RIGTH = 1;
fdopc 6:9e9143e97bae 157 if(*ptr_Currnt_st_LEFT_RIGTH!=*ptr_Befr_st_LEFT_RIGTH) {
fdopc 6:9e9143e97bae 158 pwm_motorTURN.write(*ptr_PWM_LEFT_RIGTH);
fdopc 6:9e9143e97bae 159 *ptr_Befr_st_LEFT_RIGTH = *ptr_Currnt_st_LEFT_RIGTH;
gcarmonar 0:4a46db9b1f3e 160 }
gcarmonar 0:4a46db9b1f3e 161
fdopc 6:9e9143e97bae 162 } else {
fdopc 6:9e9143e97bae 163 turn_right();
fdopc 6:9e9143e97bae 164 *ptr_Currnt_st_LEFT_RIGTH = 0;
fdopc 6:9e9143e97bae 165 if(*ptr_Currnt_st_LEFT_RIGTH!=*ptr_Befr_st_LEFT_RIGTH) {
fdopc 6:9e9143e97bae 166 pwm_motorTURN.write(*ptr_PWM_LEFT_RIGTH);
fdopc 6:9e9143e97bae 167 *ptr_Befr_st_LEFT_RIGTH = *ptr_Currnt_st_LEFT_RIGTH;
fdopc 6:9e9143e97bae 168 }
fdopc 3:07a21d1d16e4 169 }
fdopc 6:9e9143e97bae 170 }
fdopc 6:9e9143e97bae 171 void stop(void)
fdopc 6:9e9143e97bae 172 {
fdopc 6:9e9143e97bae 173 motorFront_1 = 0;
fdopc 6:9e9143e97bae 174 motorFront_2 = 0;
gcarmonar 0:4a46db9b1f3e 175
fdopc 6:9e9143e97bae 176 }
fdopc 6:9e9143e97bae 177
fdopc 6:9e9143e97bae 178 void go_forward(void)
fdopc 6:9e9143e97bae 179 {
fdopc 6:9e9143e97bae 180 motorFront_1 = 1;
fdopc 6:9e9143e97bae 181 motorFront_2 = 0;
fdopc 6:9e9143e97bae 182
fdopc 6:9e9143e97bae 183 }
fdopc 6:9e9143e97bae 184 void go_reverse(void)
fdopc 6:9e9143e97bae 185 {
fdopc 6:9e9143e97bae 186 motorFront_1 = 0;
fdopc 6:9e9143e97bae 187 motorFront_2 = 1;
fdopc 6:9e9143e97bae 188
fdopc 6:9e9143e97bae 189 }
fdopc 6:9e9143e97bae 190
fdopc 6:9e9143e97bae 191 void turn_left(void)
fdopc 6:9e9143e97bae 192 {
fdopc 6:9e9143e97bae 193 motorTurn_1 = 0;
fdopc 6:9e9143e97bae 194 motorTurn_2 = 1;
fdopc 6:9e9143e97bae 195 }
fdopc 6:9e9143e97bae 196 void turn_right(void)
fdopc 6:9e9143e97bae 197 {
fdopc 6:9e9143e97bae 198 motorTurn_1 = 1;
fdopc 6:9e9143e97bae 199 motorTurn_2 = 0;
fdopc 6:9e9143e97bae 200 }
fdopc 6:9e9143e97bae 201 void turn_none(void)
fdopc 6:9e9143e97bae 202 {
fdopc 6:9e9143e97bae 203 motorTurn_1 = 0;
fdopc 6:9e9143e97bae 204 motorTurn_2 = 0;
gcarmonar 0:4a46db9b1f3e 205 }