ROBOTIC CHAMPION TEAM / Mbed 2 deprecated Robotics_motor_no_serve

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

Fork of Robotics_LAB_motor by NTHUPME_Robotics_2017

main.cpp@1:d0c9519c70eb, 2017-03-24 (annotated)

Committer:
tea5062001
Date:
Fri Mar 24 04:36:58 2017 +0000
Revision:
1:d0c9519c70eb
Parent:
0:74ea99c4db88 
1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hoting 0:74ea99c4db88 1 #include "mbed.h"
hoting 0:74ea99c4db88 2
hoting 0:74ea99c4db88 3 //The number will be compiled as type "double" in default
hoting 0:74ea99c4db88 4 //Add a "f" after the number can make it compiled as type "float"
hoting 0:74ea99c4db88 5 #define Ts 0.01f //period of timer1 (s)
hoting 0:74ea99c4db88 6
hoting 0:74ea99c4db88 7 Ticker timer1;
hoting 0:74ea99c4db88 8 // servo motor
hoting 0:74ea99c4db88 9 PwmOut servo_cmd(A0);
hoting 0:74ea99c4db88 10 // DC motor
hoting 0:74ea99c4db88 11 PwmOut pwm1(D7);
hoting 0:74ea99c4db88 12 PwmOut pwm1n(D11);
hoting 0:74ea99c4db88 13 PwmOut pwm2(D8);
hoting 0:74ea99c4db88 14 PwmOut pwm2n(A3);
hoting 0:74ea99c4db88 15
hoting 0:74ea99c4db88 16 // Motor1 sensor
hoting 0:74ea99c4db88 17 InterruptIn HallA(A1);
hoting 0:74ea99c4db88 18 InterruptIn HallB(A2);
hoting 0:74ea99c4db88 19 // Motor2 sensor
hoting 0:74ea99c4db88 20 InterruptIn HallA_2(D13);
hoting 0:74ea99c4db88 21 InterruptIn HallB_2(D12);
hoting 0:74ea99c4db88 22
hoting 0:74ea99c4db88 23 // 函式宣告
hoting 0:74ea99c4db88 24 void init_IO();
hoting 0:74ea99c4db88 25 void init_TIMER();
hoting 0:74ea99c4db88 26 void timer1_ITR();
hoting 0:74ea99c4db88 27 void init_CN();
hoting 0:74ea99c4db88 28 void CN_ITR();
hoting 0:74ea99c4db88 29 void init_PWM();
hoting 0:74ea99c4db88 30
hoting 0:74ea99c4db88 31 // servo motor
tea5062001 1:d0c9519c70eb 32 float servo_duty = 0.069; // 0.069 +(0.088/180)*angle, -90<angle<90
hoting 0:74ea99c4db88 33 // 90度->duty=0.025; 0度->duty=0.069; -90度->duty=0.113
hoting 0:74ea99c4db88 34 int angle = 0;
hoting 0:74ea99c4db88 35
hoting 0:74ea99c4db88 36 // Hall sensor
hoting 0:74ea99c4db88 37 int HallA_1_state = 0;
hoting 0:74ea99c4db88 38 int HallB_1_state = 0;
hoting 0:74ea99c4db88 39 int state_1 = 0;
hoting 0:74ea99c4db88 40 int state_1_old = 0;
hoting 0:74ea99c4db88 41 int HallA_2_state = 0;
hoting 0:74ea99c4db88 42 int HallB_2_state = 0;
hoting 0:74ea99c4db88 43 int state_2 = 0;
hoting 0:74ea99c4db88 44 int state_2_old = 0;
hoting 0:74ea99c4db88 45
hoting 0:74ea99c4db88 46 // DC motor rotation speed control
hoting 0:74ea99c4db88 47 int speed_count_1 = 0;
hoting 0:74ea99c4db88 48 float rotation_speed_1 = 0.0;
hoting 0:74ea99c4db88 49 float rotation_speed_ref_1 = 0;
hoting 0:74ea99c4db88 50 float pwm1_duty = 0.5;
hoting 0:74ea99c4db88 51 float PI_out_1 = 0.0;
hoting 0:74ea99c4db88 52 float err_1 = 0.0;
hoting 0:74ea99c4db88 53 float ierr_1 = 0.0;
hoting 0:74ea99c4db88 54 int speed_count_2 = 0;
hoting 0:74ea99c4db88 55 float rotation_speed_2 = 0.0;
hoting 0:74ea99c4db88 56 float rotation_speed_ref_2 = 0;
hoting 0:74ea99c4db88 57 float pwm2_duty = 0.5;
hoting 0:74ea99c4db88 58 float PI_out_2 = 0.0;
hoting 0:74ea99c4db88 59 float err_2 = 0.0;
hoting 0:74ea99c4db88 60 float ierr_2 = 0.0;
hoting 0:74ea99c4db88 61
tea5062001 1:d0c9519c70eb 62 float Kp = 0.006;
tea5062001 1:d0c9519c70eb 63 float Ki = 0.02;
tea5062001 1:d0c9519c70eb 64
hoting 0:74ea99c4db88 65 int main()
hoting 0:74ea99c4db88 66 {
hoting 0:74ea99c4db88 67 init_TIMER();
hoting 0:74ea99c4db88 68 init_PWM();
hoting 0:74ea99c4db88 69 init_CN();
hoting 0:74ea99c4db88 70
hoting 0:74ea99c4db88 71 while(1) {
hoting 0:74ea99c4db88 72 }
hoting 0:74ea99c4db88 73 }
hoting 0:74ea99c4db88 74
hoting 0:74ea99c4db88 75 void init_TIMER()
hoting 0:74ea99c4db88 76 {
hoting 0:74ea99c4db88 77 timer1.attach_us(&timer1_ITR, 10000.0); // the address of the function to be attached (timer1_ITR) and the interval (10000 micro-seconds)
hoting 0:74ea99c4db88 78 }
hoting 0:74ea99c4db88 79
hoting 0:74ea99c4db88 80 void init_PWM()
hoting 0:74ea99c4db88 81 {
hoting 0:74ea99c4db88 82 servo_cmd.period_ms(20);
hoting 0:74ea99c4db88 83 servo_cmd.write(servo_duty);
hoting 0:74ea99c4db88 84
hoting 0:74ea99c4db88 85 pwm1.period_us(50);
hoting 0:74ea99c4db88 86 pwm1.write(0.5);
hoting 0:74ea99c4db88 87 TIM1->CCER |= 0x4;
hoting 0:74ea99c4db88 88
hoting 0:74ea99c4db88 89 pwm2.period_us(50);
hoting 0:74ea99c4db88 90 pwm2.write(0.5);
hoting 0:74ea99c4db88 91 TIM1->CCER |= 0x40;
hoting 0:74ea99c4db88 92 }
hoting 0:74ea99c4db88 93 void init_CN()
hoting 0:74ea99c4db88 94 {
hoting 0:74ea99c4db88 95 HallA.rise(&CN_ITR);
hoting 0:74ea99c4db88 96 HallA.fall(&CN_ITR);
hoting 0:74ea99c4db88 97 HallB.rise(&CN_ITR);
hoting 0:74ea99c4db88 98 HallB.fall(&CN_ITR);
hoting 0:74ea99c4db88 99
hoting 0:74ea99c4db88 100 HallA_2.rise(&CN_ITR);
hoting 0:74ea99c4db88 101 HallA_2.fall(&CN_ITR);
hoting 0:74ea99c4db88 102 HallB_2.rise(&CN_ITR);
hoting 0:74ea99c4db88 103 HallB_2.fall(&CN_ITR);
hoting 0:74ea99c4db88 104 }
hoting 0:74ea99c4db88 105
hoting 0:74ea99c4db88 106 void CN_ITR()
hoting 0:74ea99c4db88 107 {
hoting 0:74ea99c4db88 108 // motor1
hoting 0:74ea99c4db88 109 HallA_1_state = HallA.read();
hoting 0:74ea99c4db88 110 HallB_1_state = HallB.read();
hoting 0:74ea99c4db88 111
hoting 0:74ea99c4db88 112 ///code for state determination///
tea5062001 1:d0c9519c70eb 113 state_1 = 10*HallA_1_state + HallB_1_state; //state = AB (ex:A=1,B=0, state_1 = 10)
tea5062001 1:d0c9519c70eb 114
tea5062001 1:d0c9519c70eb 115 if(state_1_old != state_1)
tea5062001 1:d0c9519c70eb 116 {
tea5062001 1:d0c9519c70eb 117 if((state_1_old/10) == (state_1_old%10))
tea5062001 1:d0c9519c70eb 118 {
tea5062001 1:d0c9519c70eb 119 if((state_1%10) != (state_1_old%10))
tea5062001 1:d0c9519c70eb 120 {
tea5062001 1:d0c9519c70eb 121 speed_count_1++;
tea5062001 1:d0c9519c70eb 122 }
tea5062001 1:d0c9519c70eb 123 else if((state_1/10) != (state_1_old/10))
tea5062001 1:d0c9519c70eb 124 {
tea5062001 1:d0c9519c70eb 125 speed_count_1--;
tea5062001 1:d0c9519c70eb 126 }
tea5062001 1:d0c9519c70eb 127 }
tea5062001 1:d0c9519c70eb 128 else if((state_1_old/10) != (state_1_old%10))
tea5062001 1:d0c9519c70eb 129 {
tea5062001 1:d0c9519c70eb 130 if((state_1%10) != (state_1_old%10))
tea5062001 1:d0c9519c70eb 131 {
tea5062001 1:d0c9519c70eb 132 speed_count_1--;
tea5062001 1:d0c9519c70eb 133 }
tea5062001 1:d0c9519c70eb 134 else if((state_1/10) != (state_1_old/10))
tea5062001 1:d0c9519c70eb 135 {
tea5062001 1:d0c9519c70eb 136 speed_count_1++;
tea5062001 1:d0c9519c70eb 137 }
tea5062001 1:d0c9519c70eb 138 }
tea5062001 1:d0c9519c70eb 139
tea5062001 1:d0c9519c70eb 140 state_1_old = state_1;
tea5062001 1:d0c9519c70eb 141 }
tea5062001 1:d0c9519c70eb 142
hoting 0:74ea99c4db88 143 //////////////////////////////////
hoting 0:74ea99c4db88 144
hoting 0:74ea99c4db88 145 //forward : speed_count_1 + 1
hoting 0:74ea99c4db88 146 //backward : speed_count_1 - 1
hoting 0:74ea99c4db88 147
hoting 0:74ea99c4db88 148
hoting 0:74ea99c4db88 149 // motor2
hoting 0:74ea99c4db88 150 HallA_2_state = HallA_2.read();
hoting 0:74ea99c4db88 151 HallB_2_state = HallB_2.read();
hoting 0:74ea99c4db88 152
hoting 0:74ea99c4db88 153 ///code for state determination///
tea5062001 1:d0c9519c70eb 154 state_2 = 10*HallA_2_state + HallB_2_state; //state = AB (ex:A=1,B=0, state_1 = 10)
tea5062001 1:d0c9519c70eb 155
tea5062001 1:d0c9519c70eb 156 if(state_2_old != state_2)
tea5062001 1:d0c9519c70eb 157 {
tea5062001 1:d0c9519c70eb 158 if((state_2_old/10) == (state_2_old%10))
tea5062001 1:d0c9519c70eb 159 {
tea5062001 1:d0c9519c70eb 160 if((state_2%10) != (state_2_old%10))
tea5062001 1:d0c9519c70eb 161 {
tea5062001 1:d0c9519c70eb 162 speed_count_2++;
tea5062001 1:d0c9519c70eb 163 }
tea5062001 1:d0c9519c70eb 164 else if((state_2/10) != (state_2_old/10))
tea5062001 1:d0c9519c70eb 165 {
tea5062001 1:d0c9519c70eb 166 speed_count_2--;
tea5062001 1:d0c9519c70eb 167 }
tea5062001 1:d0c9519c70eb 168 }
tea5062001 1:d0c9519c70eb 169 else if((state_2_old/10) != (state_2_old%10))
tea5062001 1:d0c9519c70eb 170 {
tea5062001 1:d0c9519c70eb 171 if((state_2%10) != (state_2_old%10))
tea5062001 1:d0c9519c70eb 172 {
tea5062001 1:d0c9519c70eb 173 speed_count_2--;
tea5062001 1:d0c9519c70eb 174 }
tea5062001 1:d0c9519c70eb 175 else if((state_2/10) != (state_2_old/10))
tea5062001 1:d0c9519c70eb 176 {
tea5062001 1:d0c9519c70eb 177 speed_count_2++;
tea5062001 1:d0c9519c70eb 178 }
tea5062001 1:d0c9519c70eb 179 }
tea5062001 1:d0c9519c70eb 180
tea5062001 1:d0c9519c70eb 181 state_2_old = state_2;
tea5062001 1:d0c9519c70eb 182 }
hoting 0:74ea99c4db88 183 //////////////////////////////////
hoting 0:74ea99c4db88 184
hoting 0:74ea99c4db88 185 //forward : speed_count_2 + 1
hoting 0:74ea99c4db88 186 //backward : speed_count_2 - 1
hoting 0:74ea99c4db88 187 }
hoting 0:74ea99c4db88 188
hoting 0:74ea99c4db88 189 void timer1_ITR()
hoting 0:74ea99c4db88 190 {
hoting 0:74ea99c4db88 191 // servo motor
hoting 0:74ea99c4db88 192 ///code for sevo motor///
hoting 0:74ea99c4db88 193
tea5062001 1:d0c9519c70eb 194 servo_duty += 11.0f/1500.0f;
hoting 0:74ea99c4db88 195
hoting 0:74ea99c4db88 196 /////////////////////////
hoting 0:74ea99c4db88 197
hoting 0:74ea99c4db88 198 if(servo_duty >= 0.113f)servo_duty = 0.113;
hoting 0:74ea99c4db88 199 else if(servo_duty <= 0.025f)servo_duty = 0.025;
hoting 0:74ea99c4db88 200 servo_cmd.write(servo_duty);
hoting 0:74ea99c4db88 201
hoting 0:74ea99c4db88 202 // motor1
hoting 0:74ea99c4db88 203 rotation_speed_1 = (float)speed_count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
hoting 0:74ea99c4db88 204 speed_count_1 = 0;
hoting 0:74ea99c4db88 205
hoting 0:74ea99c4db88 206 ///PI controller for motor1///
tea5062001 1:d0c9519c70eb 207 rotation_speed_ref_1 = 150;
tea5062001 1:d0c9519c70eb 208 err_1 = rotation_speed_ref_1 - rotation_speed_1;
tea5062001 1:d0c9519c70eb 209 ierr_1 = ierr_1 + err_1 * Ts;
tea5062001 1:d0c9519c70eb 210 PI_out_1 = Kp * err_1 + Ki * ierr_1;
hoting 0:74ea99c4db88 211
hoting 0:74ea99c4db88 212 //////////////////////////////
hoting 0:74ea99c4db88 213
hoting 0:74ea99c4db88 214 if(PI_out_1 >= 0.5f)PI_out_1 = 0.5;
hoting 0:74ea99c4db88 215 else if(PI_out_1 <= -0.5f)PI_out_1 = -0.5;
hoting 0:74ea99c4db88 216 pwm1_duty = PI_out_1 + 0.5f;
hoting 0:74ea99c4db88 217 pwm1.write(pwm1_duty);
hoting 0:74ea99c4db88 218 TIM1->CCER |= 0x4;
hoting 0:74ea99c4db88 219
hoting 0:74ea99c4db88 220 //motor2
hoting 0:74ea99c4db88 221 rotation_speed_2 = (float)speed_count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //unit: rpm
hoting 0:74ea99c4db88 222 speed_count_2 = 0;
hoting 0:74ea99c4db88 223
hoting 0:74ea99c4db88 224 ///PI controller for motor2///
tea5062001 1:d0c9519c70eb 225 rotation_speed_ref_2 = 150;
tea5062001 1:d0c9519c70eb 226 err_2 = rotation_speed_ref_2 - rotation_speed_2;
tea5062001 1:d0c9519c70eb 227 ierr_2 = ierr_2 + err_2 * Ts;
tea5062001 1:d0c9519c70eb 228 PI_out_2 = Kp * err_2 + Ki * ierr_2;
hoting 0:74ea99c4db88 229
hoting 0:74ea99c4db88 230
hoting 0:74ea99c4db88 231 //////////////////////////////
hoting 0:74ea99c4db88 232
hoting 0:74ea99c4db88 233 if(PI_out_2 >= 0.5f)PI_out_2 = 0.5;
hoting 0:74ea99c4db88 234 else if(PI_out_2 <= -0.5f)PI_out_2 = -0.5;
hoting 0:74ea99c4db88 235 pwm2_duty = PI_out_2 + 0.5f;
hoting 0:74ea99c4db88 236 pwm2.write(pwm2_duty);
hoting 0:74ea99c4db88 237 TIM1->CCER |= 0x40;
hoting 0:74ea99c4db88 238 }