Robotics Term Project / Mbed 2 deprecated Robottics_Motion

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

AI_Friday.cpp@12:c1a667ca6c53, 2016-05-25 (annotated)

Committer:
smilestone520
Date:
Wed May 25 10:39:33 2016 +0000
Revision:
12:c1a667ca6c53
Parent:
11:44989c0bcea5
Child:
13:13d7f407d8b4
Child:
14:c46f22bcaa38
real

Who changed what in which revision?

UserRevisionLine numberNew contents of line
smilestone520 10:06cc2b1d2aaf 1 /*LAB_DCMotor*/
smilestone520 10:06cc2b1d2aaf 2 #include "mbed.h"
smilestone520 10:06cc2b1d2aaf 3
smilestone520 10:06cc2b1d2aaf 4 //The number will be compiled as type "double" in default
smilestone520 10:06cc2b1d2aaf 5 //Add a "f" after the number can make it compiled as type "float"
smilestone520 10:06cc2b1d2aaf 6 #define Ts 0.01f //period of timer1 (s)
smilestone520 10:06cc2b1d2aaf 7 #define Kp 0.003f
smilestone520 10:06cc2b1d2aaf 8 #define Ki 0.01f
smilestone520 10:06cc2b1d2aaf 9
smilestone520 10:06cc2b1d2aaf 10 PwmOut pwm1(D7);
smilestone520 10:06cc2b1d2aaf 11 PwmOut pwm1n(D11);
smilestone520 10:06cc2b1d2aaf 12 PwmOut pwm2(D8);
smilestone520 10:06cc2b1d2aaf 13 PwmOut pwm2n(A3);
smilestone520 10:06cc2b1d2aaf 14
smilestone520 10:06cc2b1d2aaf 15 Serial bluetooth(D10,D2);
smilestone520 10:06cc2b1d2aaf 16 Serial pc(D1, D0);
smilestone520 10:06cc2b1d2aaf 17
smilestone520 10:06cc2b1d2aaf 18 DigitalOut led1(A4);
smilestone520 10:06cc2b1d2aaf 19 DigitalOut led2(A5);
smilestone520 10:06cc2b1d2aaf 20
smilestone520 10:06cc2b1d2aaf 21 //Motor1 sensor
smilestone520 10:06cc2b1d2aaf 22 InterruptIn HallA_1(A1);
smilestone520 10:06cc2b1d2aaf 23 InterruptIn HallB_1(A2);
smilestone520 10:06cc2b1d2aaf 24 //Motor2 sensor
smilestone520 10:06cc2b1d2aaf 25 InterruptIn HallA_2(D13);
smilestone520 10:06cc2b1d2aaf 26 InterruptIn HallB_2(D12);
smilestone520 10:06cc2b1d2aaf 27
smilestone520 10:06cc2b1d2aaf 28 Ticker timer1;
smilestone520 10:06cc2b1d2aaf 29 void timer1_interrupt(void);
smilestone520 10:06cc2b1d2aaf 30 void CN_interrupt(void);
smilestone520 10:06cc2b1d2aaf 31
smilestone520 10:06cc2b1d2aaf 32 void init_TIMER(void);
smilestone520 10:06cc2b1d2aaf 33 void init_PWM(void);
smilestone520 10:06cc2b1d2aaf 34 void init_CN(void);
smilestone520 10:06cc2b1d2aaf 35
smilestone520 10:06cc2b1d2aaf 36 int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0;
smilestone520 10:06cc2b1d2aaf 37 int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0;
smilestone520 10:06cc2b1d2aaf 38
smilestone520 10:06cc2b1d2aaf 39 int v1Count = 0;
smilestone520 10:06cc2b1d2aaf 40 int v2Count = 0;
smilestone520 10:06cc2b1d2aaf 41
smilestone520 10:06cc2b1d2aaf 42 float v1 = 0.0, v1_ref = 60.0;
smilestone520 10:06cc2b1d2aaf 43 float v1_err = 0.0, v1_ierr = 0.0, PIout_1 = 0.0, PIout_1_old = 0.0;
smilestone520 10:06cc2b1d2aaf 44 float v2 = 0.0, v2_ref = -60.0;
smilestone520 10:06cc2b1d2aaf 45 float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;
smilestone520 10:06cc2b1d2aaf 46
smilestone520 11:44989c0bcea5 47 //**** receive and return by bluetooth ************** // bluetooth.getc()
smilestone520 11:44989c0bcea5 48 float xC, yC // car's position
smilestone520 11:44989c0bcea5 49
smilestone520 11:44989c0bcea5 50 float angleC // car's angle
smilestone520 11:44989c0bcea5 51
smilestone520 11:44989c0bcea5 52 float xP,yP //position that car need to reach
smilestone520 11:44989c0bcea5 53
smilestone520 11:44989c0bcea5 54 float angleR // angleR = car-nextSpot direction ---> car direction
smilestone520 11:44989c0bcea5 55
smilestone520 11:44989c0bcea5 56 int aI_State = 0;
smilestone520 11:44989c0bcea5 57
smilestone520 10:06cc2b1d2aaf 58 int main() {
smilestone520 10:06cc2b1d2aaf 59
smilestone520 10:06cc2b1d2aaf 60 init_TIMER();
smilestone520 10:06cc2b1d2aaf 61 init_PWM();
smilestone520 10:06cc2b1d2aaf 62 init_CN();
smilestone520 10:06cc2b1d2aaf 63
smilestone520 10:06cc2b1d2aaf 64 bluetooth.baud(115200); //設定鮑率
smilestone520 10:06cc2b1d2aaf 65 pc.baud(57600);
smilestone520 10:06cc2b1d2aaf 66
smilestone520 10:06cc2b1d2aaf 67
smilestone520 10:06cc2b1d2aaf 68 while(1)
smilestone520 10:06cc2b1d2aaf 69 {
smilestone520 10:06cc2b1d2aaf 70 if(pc.readable())
smilestone520 10:06cc2b1d2aaf 71 {
smilestone520 10:06cc2b1d2aaf 72 bluetooth.putc(pc.getc());
smilestone520 10:06cc2b1d2aaf 73 }
smilestone520 10:06cc2b1d2aaf 74 if(bluetooth.readable())
smilestone520 10:06cc2b1d2aaf 75 {
smilestone520 10:06cc2b1d2aaf 76 pc.putc(bluetooth.getc());
smilestone520 10:06cc2b1d2aaf 77 }
smilestone520 10:06cc2b1d2aaf 78 }
smilestone520 10:06cc2b1d2aaf 79 }
smilestone520 10:06cc2b1d2aaf 80
smilestone520 10:06cc2b1d2aaf 81 void timer1_interrupt(void)
smilestone520 10:06cc2b1d2aaf 82 {
smilestone520 10:06cc2b1d2aaf 83 //Motor 1
smilestone520 10:06cc2b1d2aaf 84 v1 = (float)v1Count * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
smilestone520 10:06cc2b1d2aaf 85 v1Count = 0;
smilestone520 10:06cc2b1d2aaf 86
smilestone520 10:06cc2b1d2aaf 87 ///code for PI control///
smilestone520 10:06cc2b1d2aaf 88 v1_err = v1_ref - v1;
smilestone520 10:06cc2b1d2aaf 89 v1_ierr = 0.01f*v1_err + v1_ierr;
smilestone520 10:06cc2b1d2aaf 90 PIout_1 = Kp*v1_err + Ki*v1_ierr;
smilestone520 10:06cc2b1d2aaf 91
smilestone520 10:06cc2b1d2aaf 92 /////////////////////////
smilestone520 10:06cc2b1d2aaf 93
smilestone520 10:06cc2b1d2aaf 94 if(PIout_1 >= 0.5f)PIout_1 = 0.5f;
smilestone520 10:06cc2b1d2aaf 95 else if(PIout_1 <= -0.5f)PIout_1 = -0.5f;
smilestone520 10:06cc2b1d2aaf 96 pwm1.write(PIout_1 + 0.5f);
smilestone520 10:06cc2b1d2aaf 97 TIM1->CCER |= 0x4;
smilestone520 10:06cc2b1d2aaf 98
smilestone520 10:06cc2b1d2aaf 99
smilestone520 10:06cc2b1d2aaf 100 //Motor 2
smilestone520 10:06cc2b1d2aaf 101 v2 = (float)v2Count * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
smilestone520 10:06cc2b1d2aaf 102 v2Count = 0;
smilestone520 10:06cc2b1d2aaf 103
smilestone520 10:06cc2b1d2aaf 104 ///code for PI control///
smilestone520 10:06cc2b1d2aaf 105 v2_err = v2_ref - v2;
smilestone520 10:06cc2b1d2aaf 106 v2_ierr = 0.01f*v2_err + v2_ierr;
smilestone520 10:06cc2b1d2aaf 107 PIout_2 = Kp*v2_err + Ki*v2_ierr;
smilestone520 10:06cc2b1d2aaf 108
smilestone520 10:06cc2b1d2aaf 109 /////////////////////////
smilestone520 10:06cc2b1d2aaf 110
smilestone520 10:06cc2b1d2aaf 111 if(PIout_2 >= 0.5f)PIout_2 = 0.5f;
smilestone520 10:06cc2b1d2aaf 112 else if(PIout_2 <= -0.5f)PIout_2 = -0.5f;
smilestone520 10:06cc2b1d2aaf 113 pwm2.write(PIout_2 + 0.5f);
smilestone520 10:06cc2b1d2aaf 114 TIM1->CCER |= 0x40;
smilestone520 10:06cc2b1d2aaf 115
smilestone520 11:44989c0bcea5 116
smilestone520 11:44989c0bcea5 117
smilestone520 11:44989c0bcea5 118 //***** main AI **************************
smilestone520 11:44989c0bcea5 119
smilestone520 11:44989c0bcea5 120
smilestone520 11:44989c0bcea5 121 //***** get position information from the bluetooth
smilestone520 11:44989c0bcea5 122 xC = ;
smilestone520 11:44989c0bcea5 123 yC = ; // car's position
smilestone520 11:44989c0bcea5 124
smilestone520 11:44989c0bcea5 125 angleC = ; // car's angle
smilestone520 11:44989c0bcea5 126
smilestone520 11:44989c0bcea5 127 xP = ;
smilestone520 11:44989c0bcea5 128 yP = ;//position that car need to reach
smilestone520 11:44989c0bcea5 129
smilestone520 11:44989c0bcea5 130 angleR = ; // angleR = car-nextSpot direction ---> car direction
smilestone520 11:44989c0bcea5 131
smilestone520 11:44989c0bcea5 132 //**** AI_State ********************
smilestone520 11:44989c0bcea5 133
smilestone520 11:44989c0bcea5 134 switch(aI_State)
smilestone520 11:44989c0bcea5 135 {
smilestone520 12:c1a667ca6c53 136 case 0: // IDLE
smilestone520 12:c1a667ca6c53 137 // IDLE check if stop color appear
smilestone520 12:c1a667ca6c53 138 //check purple color appear or not
smilestone520 12:c1a667ca6c53 139
smilestone520 12:c1a667ca6c53 140 setSpecs();
smilestone520 11:44989c0bcea5 141 break;
smilestone520 12:c1a667ca6c53 142 case 1: // get first information
smilestone520 12:c1a667ca6c53 143 // set everything icludes the car's and ball's position
smilestone520 12:c1a667ca6c53 144 // go to case 2 after get all the information
smilestone520 11:44989c0bcea5 145 break;
smilestone520 12:c1a667ca6c53 146 case 2: /// border condition
smilestone520 12:c1a667ca6c53 147 // check if car fit border conditions
smilestone520 12:c1a667ca6c53 148 funcBorder();
smilestone520 12:c1a667ca6c53 149 to case3
smilestone520 11:44989c0bcea5 150 break;
smilestone520 12:c1a667ca6c53 151 case 3://move to get ball
smilestone520 12:c1a667ca6c53 152
smilestone520 12:c1a667ca6c53 153 getBall();
smilestone520 11:44989c0bcea5 154 break;
smilestone520 12:c1a667ca6c53 155 case 4: /// move to the gate and release ball
smilestone520 12:c1a667ca6c53 156 // move to the point in front of the gate first
smilestone520 12:c1a667ca6c53 157 // then head to the gate
smilestone520 12:c1a667ca6c53 158 break;
smilestone520 12:c1a667ca6c53 159 case 5:
smilestone520 12:c1a667ca6c53 160 break;
smilestone520 11:44989c0bcea5 161
smilestone520 11:44989c0bcea5 162 }
smilestone520 11:44989c0bcea5 163
smilestone520 11:44989c0bcea5 164
smilestone520 11:44989c0bcea5 165
smilestone520 11:44989c0bcea5 166
smilestone520 11:44989c0bcea5 167
smilestone520 11:44989c0bcea5 168
smilestone520 10:06cc2b1d2aaf 169 switch(bluetooth.getc())
smilestone520 10:06cc2b1d2aaf 170 {
smilestone520 10:06cc2b1d2aaf 171 case '1':
smilestone520 10:06cc2b1d2aaf 172 v1_ref = 30;
smilestone520 10:06cc2b1d2aaf 173 v2_ref = -30;
smilestone520 10:06cc2b1d2aaf 174 break;
smilestone520 10:06cc2b1d2aaf 175 case '2':
smilestone520 10:06cc2b1d2aaf 176 v1_ref = 40;
smilestone520 10:06cc2b1d2aaf 177 v2_ref = -40;
smilestone520 10:06cc2b1d2aaf 178 break;
smilestone520 10:06cc2b1d2aaf 179 case '3':
smilestone520 10:06cc2b1d2aaf 180 v1_ref = 50;
smilestone520 10:06cc2b1d2aaf 181 v2_ref = -50;
smilestone520 10:06cc2b1d2aaf 182 break;
smilestone520 10:06cc2b1d2aaf 183 case '4':
smilestone520 10:06cc2b1d2aaf 184 v1_ref = 60;
smilestone520 10:06cc2b1d2aaf 185 v2_ref = -60;
smilestone520 10:06cc2b1d2aaf 186 break;
smilestone520 10:06cc2b1d2aaf 187 case '5':
smilestone520 10:06cc2b1d2aaf 188 v1_ref = -70;
smilestone520 10:06cc2b1d2aaf 189 v2_ref = 70;
smilestone520 10:06cc2b1d2aaf 190 break;
smilestone520 10:06cc2b1d2aaf 191 case '6':
smilestone520 10:06cc2b1d2aaf 192 v1_ref = -80;
smilestone520 10:06cc2b1d2aaf 193 v2_ref = 80;
smilestone520 10:06cc2b1d2aaf 194 break;
smilestone520 10:06cc2b1d2aaf 195 case '7':
smilestone520 10:06cc2b1d2aaf 196 v1_ref = -100;
smilestone520 10:06cc2b1d2aaf 197 v2_ref = 100;
smilestone520 10:06cc2b1d2aaf 198 break;
smilestone520 10:06cc2b1d2aaf 199 case '8':
smilestone520 10:06cc2b1d2aaf 200 v1_ref = 0;
smilestone520 10:06cc2b1d2aaf 201 v2_ref = 0;
smilestone520 10:06cc2b1d2aaf 202 break;
smilestone520 10:06cc2b1d2aaf 203 }
smilestone520 10:06cc2b1d2aaf 204
smilestone520 10:06cc2b1d2aaf 205
smilestone520 10:06cc2b1d2aaf 206 }
smilestone520 10:06cc2b1d2aaf 207
smilestone520 10:06cc2b1d2aaf 208 void CN_interrupt(void)
smilestone520 10:06cc2b1d2aaf 209 {
smilestone520 10:06cc2b1d2aaf 210 //Motor 1
smilestone520 10:06cc2b1d2aaf 211 stateA_1 = HallA_1.read();
smilestone520 10:06cc2b1d2aaf 212 stateB_1 = HallB_1.read();
smilestone520 10:06cc2b1d2aaf 213
smilestone520 10:06cc2b1d2aaf 214 ///code for state determination///
smilestone520 10:06cc2b1d2aaf 215 if(stateA_1==0&&stateB_1==0){
smilestone520 10:06cc2b1d2aaf 216 state_1 = 1;}
smilestone520 10:06cc2b1d2aaf 217 else if(stateA_1==0&&stateB_1==1){
smilestone520 10:06cc2b1d2aaf 218 state_1 = 2;}
smilestone520 10:06cc2b1d2aaf 219 else if(stateA_1==1&&stateB_1==1){
smilestone520 10:06cc2b1d2aaf 220 state_1 = 3;}
smilestone520 10:06cc2b1d2aaf 221 else if(stateA_1==1&&stateB_1==0){
smilestone520 10:06cc2b1d2aaf 222 state_1 = 4;}
smilestone520 10:06cc2b1d2aaf 223
smilestone520 10:06cc2b1d2aaf 224 if(state_1 == 1)
smilestone520 10:06cc2b1d2aaf 225 {
smilestone520 10:06cc2b1d2aaf 226 if(state_1-state_1_old == -3)
smilestone520 10:06cc2b1d2aaf 227 v1Count=v1Count+1;
smilestone520 10:06cc2b1d2aaf 228 else if(state_1-state_1_old == -1)
smilestone520 10:06cc2b1d2aaf 229 v1Count=v1Count-1;
smilestone520 10:06cc2b1d2aaf 230 }
smilestone520 10:06cc2b1d2aaf 231 else if(state_1 == 2)
smilestone520 10:06cc2b1d2aaf 232 {
smilestone520 10:06cc2b1d2aaf 233 if(state_1-state_1_old == 1)
smilestone520 10:06cc2b1d2aaf 234 v1Count=v1Count+1;
smilestone520 10:06cc2b1d2aaf 235 else if(state_1-state_1_old == -1)
smilestone520 10:06cc2b1d2aaf 236 v1Count=v1Count-1;
smilestone520 10:06cc2b1d2aaf 237 }
smilestone520 10:06cc2b1d2aaf 238 else if(state_1 == 3)
smilestone520 10:06cc2b1d2aaf 239 {
smilestone520 10:06cc2b1d2aaf 240 if(state_1-state_1_old == 1)
smilestone520 10:06cc2b1d2aaf 241 v1Count=v1Count+1;
smilestone520 10:06cc2b1d2aaf 242 else if(state_1-state_1_old == -1)
smilestone520 10:06cc2b1d2aaf 243 v1Count=v1Count-1;
smilestone520 10:06cc2b1d2aaf 244 }
smilestone520 10:06cc2b1d2aaf 245 else if(state_1 == 4)
smilestone520 10:06cc2b1d2aaf 246 {
smilestone520 10:06cc2b1d2aaf 247 if(state_1-state_1_old == 1)
smilestone520 10:06cc2b1d2aaf 248 v1Count=v1Count+1;
smilestone520 10:06cc2b1d2aaf 249 else if(state_1-state_1_old == 3)
smilestone520 10:06cc2b1d2aaf 250 v1Count=v1Count-1;
smilestone520 10:06cc2b1d2aaf 251 }
smilestone520 10:06cc2b1d2aaf 252 state_1_old = state_1;
smilestone520 10:06cc2b1d2aaf 253
smilestone520 10:06cc2b1d2aaf 254
smilestone520 10:06cc2b1d2aaf 255 //////////////////////////////////
smilestone520 10:06cc2b1d2aaf 256
smilestone520 10:06cc2b1d2aaf 257 //Forward
smilestone520 10:06cc2b1d2aaf 258 //v1Count +1
smilestone520 10:06cc2b1d2aaf 259 //Inverse
smilestone520 10:06cc2b1d2aaf 260 //v1Count -1
smilestone520 10:06cc2b1d2aaf 261
smilestone520 10:06cc2b1d2aaf 262
smilestone520 10:06cc2b1d2aaf 263 //Motor 2
smilestone520 10:06cc2b1d2aaf 264 stateA_2 = HallA_2.read();
smilestone520 10:06cc2b1d2aaf 265 stateB_2 = HallB_2.read();
smilestone520 10:06cc2b1d2aaf 266
smilestone520 10:06cc2b1d2aaf 267 ///code for state determination///
smilestone520 10:06cc2b1d2aaf 268 if(stateA_2==0&&stateB_2==0){
smilestone520 10:06cc2b1d2aaf 269 state_2 = 1;}
smilestone520 10:06cc2b1d2aaf 270 else if(stateA_2==0&&stateB_2==1){
smilestone520 10:06cc2b1d2aaf 271 state_2 = 2;}
smilestone520 10:06cc2b1d2aaf 272 else if(stateA_2==1&&stateB_2==1){
smilestone520 10:06cc2b1d2aaf 273 state_2 = 3;}
smilestone520 10:06cc2b1d2aaf 274 else if(stateA_2==1&&stateB_2==0){
smilestone520 10:06cc2b1d2aaf 275 state_2 = 4;}
smilestone520 10:06cc2b1d2aaf 276
smilestone520 10:06cc2b1d2aaf 277 if(state_2 == 1)
smilestone520 10:06cc2b1d2aaf 278 {
smilestone520 10:06cc2b1d2aaf 279 if(state_2-state_2_old == -3)
smilestone520 10:06cc2b1d2aaf 280 v2Count=v2Count+1;
smilestone520 10:06cc2b1d2aaf 281 else if(state_2-state_2_old == -1)
smilestone520 10:06cc2b1d2aaf 282 v2Count=v2Count-1;
smilestone520 10:06cc2b1d2aaf 283 }
smilestone520 10:06cc2b1d2aaf 284 else if(state_2 == 2)
smilestone520 10:06cc2b1d2aaf 285 {
smilestone520 10:06cc2b1d2aaf 286 if(state_2-state_2_old == 1)
smilestone520 10:06cc2b1d2aaf 287 v2Count=v2Count+1;
smilestone520 10:06cc2b1d2aaf 288 else if(state_2-state_2_old == -1)
smilestone520 10:06cc2b1d2aaf 289 v2Count=v2Count-1;
smilestone520 10:06cc2b1d2aaf 290 }
smilestone520 10:06cc2b1d2aaf 291 else if(state_2 == 3)
smilestone520 10:06cc2b1d2aaf 292 {
smilestone520 10:06cc2b1d2aaf 293 if(state_2-state_2_old == 1)
smilestone520 10:06cc2b1d2aaf 294 v2Count=v2Count+1;
smilestone520 10:06cc2b1d2aaf 295 else if(state_2-state_2_old == -1)
smilestone520 10:06cc2b1d2aaf 296 v2Count=v2Count-1;
smilestone520 10:06cc2b1d2aaf 297 }
smilestone520 10:06cc2b1d2aaf 298 else if(state_2 == 4)
smilestone520 10:06cc2b1d2aaf 299 {
smilestone520 10:06cc2b1d2aaf 300 if(state_2-state_2_old == 1)
smilestone520 10:06cc2b1d2aaf 301 v2Count=v2Count+1;
smilestone520 10:06cc2b1d2aaf 302 else if(state_2-state_2_old == 3)
smilestone520 10:06cc2b1d2aaf 303 v2Count=v2Count-1;
smilestone520 10:06cc2b1d2aaf 304 }
smilestone520 10:06cc2b1d2aaf 305 state_2_old = state_2;
smilestone520 10:06cc2b1d2aaf 306
smilestone520 10:06cc2b1d2aaf 307 //////////////////////////////////
smilestone520 10:06cc2b1d2aaf 308
smilestone520 10:06cc2b1d2aaf 309 //Forward
smilestone520 10:06cc2b1d2aaf 310 //v2Count +1
smilestone520 10:06cc2b1d2aaf 311 //Inverse
smilestone520 10:06cc2b1d2aaf 312 //v2Count -1
smilestone520 10:06cc2b1d2aaf 313 }
smilestone520 10:06cc2b1d2aaf 314
smilestone520 10:06cc2b1d2aaf 315 void init_TIMER(void)
smilestone520 10:06cc2b1d2aaf 316 {
smilestone520 10:06cc2b1d2aaf 317 timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
smilestone520 10:06cc2b1d2aaf 318 }
smilestone520 10:06cc2b1d2aaf 319
smilestone520 10:06cc2b1d2aaf 320 void init_PWM(void)
smilestone520 10:06cc2b1d2aaf 321 {
smilestone520 10:06cc2b1d2aaf 322 pwm1.period_us(50);
smilestone520 10:06cc2b1d2aaf 323 pwm1.write(0.5);
smilestone520 10:06cc2b1d2aaf 324 TIM1->CCER |= 0x4;
smilestone520 10:06cc2b1d2aaf 325
smilestone520 10:06cc2b1d2aaf 326 pwm2.period_us(50);
smilestone520 10:06cc2b1d2aaf 327 pwm2.write(0.5);
smilestone520 10:06cc2b1d2aaf 328 TIM1->CCER |= 0x40;
smilestone520 10:06cc2b1d2aaf 329 }
smilestone520 10:06cc2b1d2aaf 330
smilestone520 10:06cc2b1d2aaf 331 void init_CN(void)
smilestone520 10:06cc2b1d2aaf 332 {
smilestone520 10:06cc2b1d2aaf 333 HallA_1.rise(&CN_interrupt);
smilestone520 10:06cc2b1d2aaf 334 HallA_1.fall(&CN_interrupt);
smilestone520 10:06cc2b1d2aaf 335 HallB_1.rise(&CN_interrupt);
smilestone520 10:06cc2b1d2aaf 336 HallB_1.fall(&CN_interrupt);
smilestone520 10:06cc2b1d2aaf 337
smilestone520 10:06cc2b1d2aaf 338 HallA_2.rise(&CN_interrupt);
smilestone520 10:06cc2b1d2aaf 339 HallA_2.fall(&CN_interrupt);
smilestone520 10:06cc2b1d2aaf 340 HallB_2.rise(&CN_interrupt);
smilestone520 10:06cc2b1d2aaf 341 HallB_2.fall(&CN_interrupt);
smilestone520 10:06cc2b1d2aaf 342
smilestone520 10:06cc2b1d2aaf 343 stateA_1 = HallA_1.read();
smilestone520 10:06cc2b1d2aaf 344 stateB_1 = HallB_1.read();
smilestone520 10:06cc2b1d2aaf 345 stateA_2 = HallA_2.read();
smilestone520 10:06cc2b1d2aaf 346 stateB_2 = HallB_2.read();
smilestone520 10:06cc2b1d2aaf 347 }