Fer Pena
/
RobotV1
aaa
Fork of RobotV1 by
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?
User | Revision | Line number | New 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 | } |