NTHU機器人學 Team3 / Mbed 2 deprecated Lab4_code

code

Dependencies:   mbed

Fork of Robotics_Lab_DCMotor by NTHU_PME_Robotics_2018

main.cpp@1:6747911cdd90, 2016-04-07 (annotated)

Committer:
YCTung
Date:
Thu Apr 07 23:00:14 2016 +0000
Revision:
1:6747911cdd90
Parent:
0:0971f0666990
Child:
2:9caf46a5fe5a
add speed unit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YCTung 0:0971f0666990 1 /*LAB_DCMotor*/
YCTung 0:0971f0666990 2 #include "mbed.h"
YCTung 0:0971f0666990 3
YCTung 0:0971f0666990 4 //The number will be compiled as type "double" in default
YCTung 0:0971f0666990 5 //Add a "f" after the number can make it compiled as type "float"
YCTung 0:0971f0666990 6 #define Ts 0.01f //period of timer1 (s)
YCTung 0:0971f0666990 7
YCTung 0:0971f0666990 8 PwmOut pwm1(D7);
YCTung 0:0971f0666990 9 PwmOut pwm1n(D11);
YCTung 0:0971f0666990 10 PwmOut pwm2(D8);
YCTung 0:0971f0666990 11 PwmOut pwm2n(A3);
YCTung 0:0971f0666990 12
YCTung 0:0971f0666990 13 DigitalOut led1(A4);
YCTung 0:0971f0666990 14 DigitalOut led2(A5);
YCTung 0:0971f0666990 15
YCTung 0:0971f0666990 16 //Motor1 sensor
YCTung 0:0971f0666990 17 InterruptIn HallA_1(A1);
YCTung 0:0971f0666990 18 InterruptIn HallB_1(A2);
YCTung 0:0971f0666990 19 //Motor2 sensor
YCTung 0:0971f0666990 20 InterruptIn HallA_2(D13);
YCTung 0:0971f0666990 21 InterruptIn HallB_2(D12);
YCTung 0:0971f0666990 22
YCTung 0:0971f0666990 23 Serial pc(D1, D0);
YCTung 0:0971f0666990 24
YCTung 0:0971f0666990 25 Ticker timer1;
YCTung 0:0971f0666990 26 void timer1_interrupt(void);
YCTung 0:0971f0666990 27 void CN_interrupt(void);
YCTung 0:0971f0666990 28
YCTung 0:0971f0666990 29 void init_TIMER(void);
YCTung 0:0971f0666990 30 void init_PWM(void);
YCTung 0:0971f0666990 31 void init_CN(void);
YCTung 0:0971f0666990 32
YCTung 0:0971f0666990 33 int8_t stateA_1=0, stateB_1=0, stateA_2=0, stateB_2=0;
YCTung 0:0971f0666990 34 int8_t state_1 = 0, state_1_old = 0, state_2 = 0, state_2_old = 0;
YCTung 0:0971f0666990 35
YCTung 0:0971f0666990 36 int v1Count = 0;
YCTung 0:0971f0666990 37 int v2Count = 0;
YCTung 0:0971f0666990 38
YCTung 0:0971f0666990 39 float v1 = 0.0, v1_ref = 0.0;
YCTung 0:0971f0666990 40 float v1_err = 0.0, v1_ierr = 0.0, PIout_1 = 0.0, PIout_1_old = 0.0;
YCTung 0:0971f0666990 41 float v2 = 0.0, v2_ref = 0.0;
YCTung 0:0971f0666990 42 float v2_err = 0.0, v2_ierr = 0.0, PIout_2 = 0.0, PIout_2_old = 0.0;
YCTung 0:0971f0666990 43
YCTung 0:0971f0666990 44 int main() {
YCTung 0:0971f0666990 45
YCTung 0:0971f0666990 46 init_TIMER();
YCTung 0:0971f0666990 47 init_PWM();
YCTung 0:0971f0666990 48 init_CN();
YCTung 0:0971f0666990 49
YCTung 0:0971f0666990 50 v1_ref = 0.0;
YCTung 0:0971f0666990 51 v2_ref = 0.0;
YCTung 0:0971f0666990 52
YCTung 0:0971f0666990 53 while(1) {
YCTung 0:0971f0666990 54 ;
YCTung 0:0971f0666990 55 }
YCTung 0:0971f0666990 56 }
YCTung 0:0971f0666990 57
YCTung 0:0971f0666990 58 void timer1_interrupt(void)
YCTung 0:0971f0666990 59 {
YCTung 0:0971f0666990 60 //Motor 1
YCTung 1:6747911cdd90 61 v1 = (float)v1Count * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
YCTung 0:0971f0666990 62 v1Count = 0;
YCTung 0:0971f0666990 63
YCTung 0:0971f0666990 64 ///code for PI control///
YCTung 0:0971f0666990 65
YCTung 0:0971f0666990 66
YCTung 0:0971f0666990 67 /////////////////////////
YCTung 0:0971f0666990 68
YCTung 0:0971f0666990 69 if(PIout_1 >= 0.5f)PIout_1 = 0.5f;
YCTung 0:0971f0666990 70 else if(PIout_1 <= -0.5f)PIout_1 = -0.5f;
YCTung 0:0971f0666990 71 pwm1.write(PIout_1 + 0.5f);
YCTung 0:0971f0666990 72 TIM1->CCER |= 0x4;
YCTung 0:0971f0666990 73
YCTung 0:0971f0666990 74
YCTung 0:0971f0666990 75 //Motor 2
YCTung 1:6747911cdd90 76 v2 = (float)v2Count * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
YCTung 0:0971f0666990 77 v2Count = 0;
YCTung 0:0971f0666990 78
YCTung 0:0971f0666990 79 ///code for PI control///
YCTung 0:0971f0666990 80
YCTung 0:0971f0666990 81
YCTung 0:0971f0666990 82 /////////////////////////
YCTung 0:0971f0666990 83
YCTung 0:0971f0666990 84 if(PIout_2 >= 0.5f)PIout_2 = 0.5f;
YCTung 0:0971f0666990 85 else if(PIout_2 <= -0.5f)PIout_2 = -0.5f;
YCTung 0:0971f0666990 86 pwm2.write(PIout_2 + 0.5f);
YCTung 0:0971f0666990 87 pwm2.write(0);
YCTung 0:0971f0666990 88 TIM1->CCER |= 0x40;
YCTung 0:0971f0666990 89 }
YCTung 0:0971f0666990 90
YCTung 0:0971f0666990 91 void CN_interrupt(void)
YCTung 0:0971f0666990 92 {
YCTung 0:0971f0666990 93 //Motor 1
YCTung 0:0971f0666990 94 stateA_1 = HallA_1.read();
YCTung 0:0971f0666990 95 stateB_1 = HallB_1.read();
YCTung 0:0971f0666990 96
YCTung 0:0971f0666990 97 ///code for state determination///
YCTung 0:0971f0666990 98
YCTung 0:0971f0666990 99
YCTung 0:0971f0666990 100 //////////////////////////////////
YCTung 0:0971f0666990 101
YCTung 0:0971f0666990 102 //Forward
YCTung 0:0971f0666990 103 //v1Count +1
YCTung 0:0971f0666990 104 //Inverse
YCTung 0:0971f0666990 105 //v1Count -1
YCTung 0:0971f0666990 106
YCTung 0:0971f0666990 107
YCTung 0:0971f0666990 108 //Motor 2
YCTung 0:0971f0666990 109 stateA_2 = HallA_2.read();
YCTung 0:0971f0666990 110 stateB_2 = HallB_2.read();
YCTung 0:0971f0666990 111
YCTung 0:0971f0666990 112 ///code for state determination///
YCTung 0:0971f0666990 113
YCTung 0:0971f0666990 114
YCTung 0:0971f0666990 115 //////////////////////////////////
YCTung 0:0971f0666990 116
YCTung 0:0971f0666990 117 //Forward
YCTung 0:0971f0666990 118 //v2Count +1
YCTung 0:0971f0666990 119 //Inverse
YCTung 0:0971f0666990 120 //v2Count -1
YCTung 0:0971f0666990 121 }
YCTung 0:0971f0666990 122
YCTung 0:0971f0666990 123 void init_TIMER(void)
YCTung 0:0971f0666990 124 {
YCTung 0:0971f0666990 125 timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
YCTung 0:0971f0666990 126 }
YCTung 0:0971f0666990 127
YCTung 0:0971f0666990 128 void init_PWM(void)
YCTung 0:0971f0666990 129 {
YCTung 0:0971f0666990 130 pwm1.period_us(50);
YCTung 0:0971f0666990 131 pwm1.write(0.5);
YCTung 0:0971f0666990 132 TIM1->CCER |= 0x4;
YCTung 0:0971f0666990 133
YCTung 0:0971f0666990 134 pwm2.period_us(50);
YCTung 0:0971f0666990 135 pwm2.write(0.5);
YCTung 0:0971f0666990 136 TIM1->CCER |= 0x40;
YCTung 0:0971f0666990 137 }
YCTung 0:0971f0666990 138
YCTung 0:0971f0666990 139 void init_CN(void)
YCTung 0:0971f0666990 140 {
YCTung 0:0971f0666990 141 HallA_1.rise(&CN_interrupt);
YCTung 0:0971f0666990 142 HallA_1.fall(&CN_interrupt);
YCTung 0:0971f0666990 143 HallB_1.rise(&CN_interrupt);
YCTung 0:0971f0666990 144 HallB_1.fall(&CN_interrupt);
YCTung 0:0971f0666990 145
YCTung 0:0971f0666990 146 HallA_2.rise(&CN_interrupt);
YCTung 0:0971f0666990 147 HallA_2.fall(&CN_interrupt);
YCTung 0:0971f0666990 148 HallB_2.rise(&CN_interrupt);
YCTung 0:0971f0666990 149 HallB_2.fall(&CN_interrupt);
YCTung 0:0971f0666990 150
YCTung 0:0971f0666990 151 stateA_1 = HallA_1.read();
YCTung 0:0971f0666990 152 stateB_1 = HallB_1.read();
YCTung 0:0971f0666990 153 stateA_2 = HallA_2.read();
YCTung 0:0971f0666990 154 stateB_2 = HallB_2.read();
YCTung 0:0971f0666990 155 }