No.9 Robotics / Mbed 2 deprecated BT_Encoder

5/18

Dependencies:   mbed

main.cpp@0:16235cec088e, 2016-05-19 (annotated)

Committer:
winstonkuo
Date:
Thu May 19 12:35:02 2016 +0000
Revision:
0:16235cec088e
5/18

Who changed what in which revision?

UserRevisionLine numberNew contents of line
winstonkuo 0:16235cec088e 1 #include "mbed.h"
winstonkuo 0:16235cec088e 2
winstonkuo 0:16235cec088e 3 //The number will be compiled as type "double" in default
winstonkuo 0:16235cec088e 4 //Add a "f" after the number can make it compiled as type "float"
winstonkuo 0:16235cec088e 5 #define Ts 0.01f //period of timer1 (s)
winstonkuo 0:16235cec088e 6 #define Kp1 0.0085f
winstonkuo 0:16235cec088e 7 #define Kp2 0.0085f
winstonkuo 0:16235cec088e 8 #define Ki 0.008f
winstonkuo 0:16235cec088e 9
winstonkuo 0:16235cec088e 10 Serial bluetooth(D10,D2); //宣告藍牙腳位
winstonkuo 0:16235cec088e 11
winstonkuo 0:16235cec088e 12 PwmOut pwm1(D7);
winstonkuo 0:16235cec088e 13 PwmOut pwm1n(D11);
winstonkuo 0:16235cec088e 14 PwmOut pwm2(D8);
winstonkuo 0:16235cec088e 15 PwmOut pwm2n(A3);
winstonkuo 0:16235cec088e 16
winstonkuo 0:16235cec088e 17 DigitalOut led1(A4);
winstonkuo 0:16235cec088e 18 DigitalOut led2(A5);
winstonkuo 0:16235cec088e 19
winstonkuo 0:16235cec088e 20 //Motor1 sensor
winstonkuo 0:16235cec088e 21 InterruptIn HallA_1(A1);
winstonkuo 0:16235cec088e 22 InterruptIn HallB_1(A2);
winstonkuo 0:16235cec088e 23 //Motor2 sensor
winstonkuo 0:16235cec088e 24 InterruptIn HallA_2(D13);
winstonkuo 0:16235cec088e 25 InterruptIn HallB_2(D12);
winstonkuo 0:16235cec088e 26
winstonkuo 0:16235cec088e 27
winstonkuo 0:16235cec088e 28 Ticker timer1;
winstonkuo 0:16235cec088e 29 void timer1_interrupt(void);
winstonkuo 0:16235cec088e 30 int timer1_counter;
winstonkuo 0:16235cec088e 31
winstonkuo 0:16235cec088e 32 void CN_interrupt(void);
winstonkuo 0:16235cec088e 33
winstonkuo 0:16235cec088e 34 void init_TIMER(void);
winstonkuo 0:16235cec088e 35 void init_PWM(void);
winstonkuo 0:16235cec088e 36 void init_CN(void);
winstonkuo 0:16235cec088e 37 void init_BLUETOOTH(void);
winstonkuo 0:16235cec088e 38
winstonkuo 0:16235cec088e 39 char speedCmd;
winstonkuo 0:16235cec088e 40
winstonkuo 0:16235cec088e 41 int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0;
winstonkuo 0:16235cec088e 42 int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0;
winstonkuo 0:16235cec088e 43
winstonkuo 0:16235cec088e 44 int v1Count = 0;
winstonkuo 0:16235cec088e 45 int v2Count = 0;
winstonkuo 0:16235cec088e 46
winstonkuo 0:16235cec088e 47 float v1 = 0.0, v1_ref = 0.0;
winstonkuo 0:16235cec088e 48 float v1_err = 0.0, v1_ierr = 0.0, PIout_1 = 0.0, PIout_1_old = 0.0;
winstonkuo 0:16235cec088e 49 float v1_old[10] = {}, v1_avg = 0.0;
winstonkuo 0:16235cec088e 50 float v2 = 0.0, v2_ref = 0.0;
winstonkuo 0:16235cec088e 51 float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;
winstonkuo 0:16235cec088e 52 float v2_old[10] = {}, v2_avg = 0.0;
winstonkuo 0:16235cec088e 53
winstonkuo 0:16235cec088e 54 float s = 0.5f;
winstonkuo 0:16235cec088e 55 int main() {
winstonkuo 0:16235cec088e 56
winstonkuo 0:16235cec088e 57 init_BLUETOOTH();
winstonkuo 0:16235cec088e 58 init_TIMER();
winstonkuo 0:16235cec088e 59 init_PWM();
winstonkuo 0:16235cec088e 60 init_CN();
winstonkuo 0:16235cec088e 61
winstonkuo 0:16235cec088e 62 while(1)
winstonkuo 0:16235cec088e 63 {
winstonkuo 0:16235cec088e 64 if(bluetooth.readable())
winstonkuo 0:16235cec088e 65 {
winstonkuo 0:16235cec088e 66 speedCmd = bluetooth.getc();
winstonkuo 0:16235cec088e 67
winstonkuo 0:16235cec088e 68 switch(speedCmd)
winstonkuo 0:16235cec088e 69 {
winstonkuo 0:16235cec088e 70 case '1':
winstonkuo 0:16235cec088e 71 s = 0.8f;
winstonkuo 0:16235cec088e 72 break;
winstonkuo 0:16235cec088e 73 case '0':
winstonkuo 0:16235cec088e 74 s = 0.5f;
winstonkuo 0:16235cec088e 75 break;
winstonkuo 0:16235cec088e 76 }
winstonkuo 0:16235cec088e 77 }
winstonkuo 0:16235cec088e 78 }
winstonkuo 0:16235cec088e 79 }
winstonkuo 0:16235cec088e 80
winstonkuo 0:16235cec088e 81 void timer1_interrupt(void)
winstonkuo 0:16235cec088e 82 {
winstonkuo 0:16235cec088e 83 //Motor 1
winstonkuo 0:16235cec088e 84 v1 = (float)v1Count * 50.0f / 12.0f * 60.0f / 29.0f;
winstonkuo 0:16235cec088e 85 v1Count = 0;
winstonkuo 0:16235cec088e 86
winstonkuo 0:16235cec088e 87 pwm1.write(s);
winstonkuo 0:16235cec088e 88 TIM1->CCER |= 0x4;
winstonkuo 0:16235cec088e 89
winstonkuo 0:16235cec088e 90 //Motor 2
winstonkuo 0:16235cec088e 91 v2 = (float)v2Count * 50.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
winstonkuo 0:16235cec088e 92 v2Count = 0;
winstonkuo 0:16235cec088e 93
winstonkuo 0:16235cec088e 94 pwm2.write(s);
winstonkuo 0:16235cec088e 95 TIM1->CCER |= 0x40;
winstonkuo 0:16235cec088e 96
winstonkuo 0:16235cec088e 97 timer1_counter ++;
winstonkuo 0:16235cec088e 98 if (timer1_counter == 1)
winstonkuo 0:16235cec088e 99 {
winstonkuo 0:16235cec088e 100 timer1_counter = 0;
winstonkuo 0:16235cec088e 101 if(bluetooth.writeable())
winstonkuo 0:16235cec088e 102 {
winstonkuo 0:16235cec088e 103 bluetooth.printf("%4.2f\n",v1);
winstonkuo 0:16235cec088e 104 }
winstonkuo 0:16235cec088e 105 }
winstonkuo 0:16235cec088e 106 }
winstonkuo 0:16235cec088e 107
winstonkuo 0:16235cec088e 108 void CN_interrupt(void)
winstonkuo 0:16235cec088e 109 {
winstonkuo 0:16235cec088e 110 //Motor 1
winstonkuo 0:16235cec088e 111 stateA_1 = HallA_1.read();
winstonkuo 0:16235cec088e 112 stateB_1 = HallB_1.read();
winstonkuo 0:16235cec088e 113
winstonkuo 0:16235cec088e 114 ///code for state determination///
winstonkuo 0:16235cec088e 115 if (stateA_1 == 0)
winstonkuo 0:16235cec088e 116 {
winstonkuo 0:16235cec088e 117 if (stateB_1 == 0)
winstonkuo 0:16235cec088e 118 state_1 = 0;
winstonkuo 0:16235cec088e 119 else
winstonkuo 0:16235cec088e 120 state_1 = 1;
winstonkuo 0:16235cec088e 121 }
winstonkuo 0:16235cec088e 122 else
winstonkuo 0:16235cec088e 123 {
winstonkuo 0:16235cec088e 124 if (stateB_1 == 1)
winstonkuo 0:16235cec088e 125 state_1 = 2;
winstonkuo 0:16235cec088e 126 else
winstonkuo 0:16235cec088e 127 state_1 = 3;
winstonkuo 0:16235cec088e 128 }
winstonkuo 0:16235cec088e 129
winstonkuo 0:16235cec088e 130 //Forward: v1Count +1
winstonkuo 0:16235cec088e 131 //Inverse: v1Count -1
winstonkuo 0:16235cec088e 132 if ( (state_1 == (state_1_old + 1)) || (state_1 == 0 && state_1_old == 3) )
winstonkuo 0:16235cec088e 133 v1Count++;
winstonkuo 0:16235cec088e 134 else if ( (state_1 == (state_1_old - 1)) || (state_1 == 3 && state_1_old == 0))
winstonkuo 0:16235cec088e 135 v1Count--;
winstonkuo 0:16235cec088e 136
winstonkuo 0:16235cec088e 137 state_1_old = state_1;
winstonkuo 0:16235cec088e 138
winstonkuo 0:16235cec088e 139 //Motor 2
winstonkuo 0:16235cec088e 140 stateA_2 = HallA_2.read();
winstonkuo 0:16235cec088e 141 stateB_2 = HallB_2.read();
winstonkuo 0:16235cec088e 142
winstonkuo 0:16235cec088e 143 ///code for state determination///
winstonkuo 0:16235cec088e 144 if (stateA_2 == 0)
winstonkuo 0:16235cec088e 145 {
winstonkuo 0:16235cec088e 146 if (stateB_2 == 0)
winstonkuo 0:16235cec088e 147 state_2 = 0;
winstonkuo 0:16235cec088e 148 else
winstonkuo 0:16235cec088e 149 state_2 = 1;
winstonkuo 0:16235cec088e 150 }
winstonkuo 0:16235cec088e 151 else
winstonkuo 0:16235cec088e 152 {
winstonkuo 0:16235cec088e 153 if (stateB_2 == 1)
winstonkuo 0:16235cec088e 154 state_2 = 2;
winstonkuo 0:16235cec088e 155 else
winstonkuo 0:16235cec088e 156 state_2 = 3;
winstonkuo 0:16235cec088e 157 }
winstonkuo 0:16235cec088e 158
winstonkuo 0:16235cec088e 159 //Forward: v2Count +1
winstonkuo 0:16235cec088e 160 //Inverse: v2Count -1
winstonkuo 0:16235cec088e 161 if ( (state_2 == (state_2_old + 1)) || (state_2 == 0 && state_2_old == 3) )
winstonkuo 0:16235cec088e 162 v2Count++;
winstonkuo 0:16235cec088e 163 else if ( (state_2 == (state_2_old - 1)) || (state_2 == 3 && state_2_old == 0) )
winstonkuo 0:16235cec088e 164 v2Count--;
winstonkuo 0:16235cec088e 165
winstonkuo 0:16235cec088e 166 state_2_old = state_2;
winstonkuo 0:16235cec088e 167 }
winstonkuo 0:16235cec088e 168
winstonkuo 0:16235cec088e 169 void init_TIMER(void)
winstonkuo 0:16235cec088e 170 {
winstonkuo 0:16235cec088e 171 timer1.attach_us(&timer1_interrupt, 20000);//10ms interrupt period (100 Hz)
winstonkuo 0:16235cec088e 172 timer1_counter = 0;
winstonkuo 0:16235cec088e 173 }
winstonkuo 0:16235cec088e 174
winstonkuo 0:16235cec088e 175 void init_PWM(void)
winstonkuo 0:16235cec088e 176 {
winstonkuo 0:16235cec088e 177 pwm1.period_us(50);
winstonkuo 0:16235cec088e 178 pwm1.write(0.5);
winstonkuo 0:16235cec088e 179 TIM1->CCER |= 0x4;
winstonkuo 0:16235cec088e 180
winstonkuo 0:16235cec088e 181 pwm2.period_us(50);
winstonkuo 0:16235cec088e 182 pwm2.write(0.5);
winstonkuo 0:16235cec088e 183 TIM1->CCER |= 0x40;
winstonkuo 0:16235cec088e 184 }
winstonkuo 0:16235cec088e 185
winstonkuo 0:16235cec088e 186 void init_CN(void)
winstonkuo 0:16235cec088e 187 {
winstonkuo 0:16235cec088e 188 HallA_1.rise(&CN_interrupt);
winstonkuo 0:16235cec088e 189 HallA_1.fall(&CN_interrupt);
winstonkuo 0:16235cec088e 190 HallB_1.rise(&CN_interrupt);
winstonkuo 0:16235cec088e 191 HallB_1.fall(&CN_interrupt);
winstonkuo 0:16235cec088e 192
winstonkuo 0:16235cec088e 193 HallA_2.rise(&CN_interrupt);
winstonkuo 0:16235cec088e 194 HallA_2.fall(&CN_interrupt);
winstonkuo 0:16235cec088e 195 HallB_2.rise(&CN_interrupt);
winstonkuo 0:16235cec088e 196 HallB_2.fall(&CN_interrupt);
winstonkuo 0:16235cec088e 197
winstonkuo 0:16235cec088e 198 stateA_1 = HallA_1.read();
winstonkuo 0:16235cec088e 199 stateB_1 = HallB_1.read();
winstonkuo 0:16235cec088e 200 stateA_2 = HallA_2.read();
winstonkuo 0:16235cec088e 201 stateB_2 = HallB_2.read();
winstonkuo 0:16235cec088e 202 }
winstonkuo 0:16235cec088e 203
winstonkuo 0:16235cec088e 204 void init_BLUETOOTH(void)
winstonkuo 0:16235cec088e 205 {
winstonkuo 0:16235cec088e 206 bluetooth.baud(115200);
winstonkuo 0:16235cec088e 207 }