NTHU機器人學 Team3 / Mbed 2 deprecated Lab4_code

code

Dependencies:   mbed

Fork of Robotics_Lab_DCMotor by NTHU_PME_Robotics_2018

main.cpp@5:e9f4f76079ee, 2018-04-12 (annotated)

Committer:
KCHuang
Date:
Thu Apr 12 07:56:42 2018 +0000
Revision:
5:e9f4f76079ee
Parent:
4:3b514de5e8a8 
Lab4 code;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cpul5338 3:0856a9fa97ec 1 /* LAB DCMotor */
YCTung 0:0971f0666990 2 #include "mbed.h"
YCTung 0:0971f0666990 3
cpul5338 3:0856a9fa97ec 4 //****************************************************************************** Define
YCTung 0:0971f0666990 5 //The number will be compiled as type "double" in default
YCTung 0:0971f0666990 6 //Add a "f" after the number can make it compiled as type "float"
YCTung 0:0971f0666990 7 #define Ts 0.01f //period of timer1 (s)
KCHuang 4:3b514de5e8a8 8 #define Servo_Period 20
cpul5338 3:0856a9fa97ec 9 //****************************************************************************** End of Define
YCTung 0:0971f0666990 10
cpul5338 3:0856a9fa97ec 11 //****************************************************************************** I/O
cpul5338 3:0856a9fa97ec 12 //PWM
KCHuang 4:3b514de5e8a8 13 Serial pc(USBTX, USBRX); // tx, rx
KCHuang 4:3b514de5e8a8 14
cpul5338 3:0856a9fa97ec 15 //Dc motor
YCTung 0:0971f0666990 16 PwmOut pwm1(D7);
YCTung 0:0971f0666990 17 PwmOut pwm1n(D11);
YCTung 0:0971f0666990 18 PwmOut pwm2(D8);
YCTung 0:0971f0666990 19 PwmOut pwm2n(A3);
KCHuang 4:3b514de5e8a8 20 PwmOut servo(A0);
YCTung 0:0971f0666990 21 //Motor1 sensor
YCTung 0:0971f0666990 22 InterruptIn HallA_1(A1);
YCTung 0:0971f0666990 23 InterruptIn HallB_1(A2);
YCTung 0:0971f0666990 24 //Motor2 sensor
YCTung 0:0971f0666990 25 InterruptIn HallA_2(D13);
YCTung 0:0971f0666990 26 InterruptIn HallB_2(D12);
YCTung 0:0971f0666990 27
cpul5338 3:0856a9fa97ec 28 //LED
cpul5338 3:0856a9fa97ec 29 DigitalOut led1(A4);
cpul5338 3:0856a9fa97ec 30 DigitalOut led2(A5);
YCTung 0:0971f0666990 31
cpul5338 3:0856a9fa97ec 32 //Timer Setting
cpul5338 3:0856a9fa97ec 33 Ticker timer;
cpul5338 3:0856a9fa97ec 34 //****************************************************************************** End of I/O
cpul5338 3:0856a9fa97ec 35
cpul5338 3:0856a9fa97ec 36 //****************************************************************************** Functions
cpul5338 3:0856a9fa97ec 37 void init_timer(void);
cpul5338 3:0856a9fa97ec 38 void init_CN(void);
YCTung 0:0971f0666990 39 void init_PWM(void);
cpul5338 3:0856a9fa97ec 40 void timer_interrupt(void);
cpul5338 3:0856a9fa97ec 41 void CN_interrupt(void);
cpul5338 3:0856a9fa97ec 42 //****************************************************************************** End of Functions
YCTung 0:0971f0666990 43
cpul5338 3:0856a9fa97ec 44 //****************************************************************************** Variables
cpul5338 3:0856a9fa97ec 45 // Servo
cpul5338 3:0856a9fa97ec 46 float servo_duty = 0.066; // 0.025~0.113(-90~+90) 0.069->0 degree
cpul5338 3:0856a9fa97ec 47 // motor 1
cpul5338 3:0856a9fa97ec 48 int8_t HallA_state_1 = 0;
cpul5338 3:0856a9fa97ec 49 int8_t HallB_state_1 = 0;
cpul5338 3:0856a9fa97ec 50 int8_t motor_state_1 = 0;
cpul5338 3:0856a9fa97ec 51 int8_t motor_state_old_1 = 0;
cpul5338 3:0856a9fa97ec 52 int count_1 = 0;
cpul5338 3:0856a9fa97ec 53 float speed_1 = 0.0f;
KCHuang 4:3b514de5e8a8 54 float v_ref_1 = -80.0f;
cpul5338 3:0856a9fa97ec 55 float v_err_1 = 0.0f;
cpul5338 3:0856a9fa97ec 56 float v_ierr_1 = 0.0f;
cpul5338 3:0856a9fa97ec 57 float ctrl_output_1 = 0.0f;
cpul5338 3:0856a9fa97ec 58 float pwm1_duty = 0.0f;
cpul5338 3:0856a9fa97ec 59 //motor 2
cpul5338 3:0856a9fa97ec 60 int8_t HallA_state_2 = 0;
cpul5338 3:0856a9fa97ec 61 int8_t HallB_state_2 = 0;
cpul5338 3:0856a9fa97ec 62 int8_t motor_state_2 = 0;
cpul5338 3:0856a9fa97ec 63 int8_t motor_state_old_2 = 0;
cpul5338 3:0856a9fa97ec 64 int count_2 = 0;
cpul5338 3:0856a9fa97ec 65 float speed_2 = 0.0f;
cpul5338 3:0856a9fa97ec 66 float v_ref_2 = 150.0f;
cpul5338 3:0856a9fa97ec 67 float v_err_2 = 0.0f;
cpul5338 3:0856a9fa97ec 68 float v_ierr_2 = 0.0f;
cpul5338 3:0856a9fa97ec 69 float ctrl_output_2 = 0.0f;
cpul5338 3:0856a9fa97ec 70 float pwm2_duty = 0.0f;
KCHuang 4:3b514de5e8a8 71 //servo
KCHuang 4:3b514de5e8a8 72 int i = 0;
cpul5338 3:0856a9fa97ec 73 //****************************************************************************** End of Variables
YCTung 0:0971f0666990 74
cpul5338 3:0856a9fa97ec 75 //****************************************************************************** Main
cpul5338 3:0856a9fa97ec 76 int main()
cpul5338 3:0856a9fa97ec 77 {
cpul5338 3:0856a9fa97ec 78 init_timer();
YCTung 0:0971f0666990 79 init_PWM();
YCTung 0:0971f0666990 80 init_CN();
cpul5338 3:0856a9fa97ec 81 while(1)
roger5641 2:9caf46a5fe5a 82 {
KCHuang 4:3b514de5e8a8 83 pc.printf("**************************\n");
KCHuang 4:3b514de5e8a8 84 pc.printf("speed1: %f, error: %f\n",speed_1,v_err_1);
KCHuang 4:3b514de5e8a8 85 pc.printf("i_error: %f ctrl output: %f \n",v_ierr_1,ctrl_output_1);
KCHuang 4:3b514de5e8a8 86 pc.printf("speed2: %f, error: %f\n",speed_2,v_err_2);
KCHuang 4:3b514de5e8a8 87 pc.printf("i_error: %f ctrl output: %f \n",v_ierr_2,ctrl_output_2);
KCHuang 4:3b514de5e8a8 88
YCTung 0:0971f0666990 89 }
YCTung 0:0971f0666990 90 }
cpul5338 3:0856a9fa97ec 91 //****************************************************************************** End of Main
YCTung 0:0971f0666990 92
cpul5338 3:0856a9fa97ec 93 //****************************************************************************** timer_interrupt
cpul5338 3:0856a9fa97ec 94 void timer_interrupt()
cpul5338 3:0856a9fa97ec 95 {
cpul5338 3:0856a9fa97ec 96 // Motor1
cpul5338 3:0856a9fa97ec 97 speed_1 = (float)count_1 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
cpul5338 3:0856a9fa97ec 98 count_1 = 0;
cpul5338 3:0856a9fa97ec 99 // Code for PI controller //
KCHuang 4:3b514de5e8a8 100 v_err_1 = v_ref_1 - speed_1;
KCHuang 4:3b514de5e8a8 101 v_ierr_1 += (v_err_1*Ts);
KCHuang 4:3b514de5e8a8 102 ctrl_output_1 = 0.01f*v_err_1+ 0.1f*v_ierr_1;
cpul5338 3:0856a9fa97ec 103 ///////////////////////////
YCTung 0:0971f0666990 104
cpul5338 3:0856a9fa97ec 105 if(ctrl_output_1 >= 0.5f)ctrl_output_1 = 0.5f;
cpul5338 3:0856a9fa97ec 106 else if(ctrl_output_1 <= -0.5f)ctrl_output_1 = -0.5f;
cpul5338 3:0856a9fa97ec 107 pwm1_duty = ctrl_output_1 + 0.5f;
cpul5338 3:0856a9fa97ec 108 pwm1.write(pwm1_duty);
YCTung 0:0971f0666990 109 TIM1->CCER |= 0x4;
YCTung 0:0971f0666990 110
cpul5338 3:0856a9fa97ec 111 // Motor2
cpul5338 3:0856a9fa97ec 112 speed_2 = (float)count_2 * 100.0f / 12.0f * 60.0f / 29.0f; //rpm
cpul5338 3:0856a9fa97ec 113 count_2 = 0;
cpul5338 3:0856a9fa97ec 114 // Code for PI controller //
KCHuang 4:3b514de5e8a8 115 v_err_2 = v_ref_2 - speed_2;
KCHuang 4:3b514de5e8a8 116 v_ierr_2 += (v_err_2*Ts);
KCHuang 4:3b514de5e8a8 117 ctrl_output_2 = 0.001f*v_err_2+ 0.05f*v_ierr_2;
KCHuang 4:3b514de5e8a8 118 ///////////////////////////
cpul5338 3:0856a9fa97ec 119 if(ctrl_output_2 >= 0.5f)ctrl_output_2 = 0.5f;
cpul5338 3:0856a9fa97ec 120 else if(ctrl_output_2 <= -0.5f)ctrl_output_2 = -0.5f;
cpul5338 3:0856a9fa97ec 121 pwm2_duty = ctrl_output_2 + 0.5f;
cpul5338 3:0856a9fa97ec 122 pwm2.write(pwm2_duty);
cpul5338 3:0856a9fa97ec 123 TIM1->CCER |= 0x40;
KCHuang 4:3b514de5e8a8 124
KCHuang 4:3b514de5e8a8 125 if(v_ierr_1 > 5)
KCHuang 4:3b514de5e8a8 126 v_ierr_1 = 0;
KCHuang 4:3b514de5e8a8 127 if(v_ierr_2 > 8)
KCHuang 4:3b514de5e8a8 128 v_ierr_2 = 0;
KCHuang 4:3b514de5e8a8 129
KCHuang 4:3b514de5e8a8 130 //Servo
KCHuang 4:3b514de5e8a8 131 if(i==100)
KCHuang 4:3b514de5e8a8 132 {
KCHuang 4:3b514de5e8a8 133 if(servo_duty < 0.08f)
KCHuang 4:3b514de5e8a8 134 {
KCHuang 4:3b514de5e8a8 135 servo_duty = servo_duty+0.0075f;
KCHuang 4:3b514de5e8a8 136 }
KCHuang 4:3b514de5e8a8 137 else
KCHuang 4:3b514de5e8a8 138 {
KCHuang 4:3b514de5e8a8 139 servo_duty = 0.08f;
KCHuang 4:3b514de5e8a8 140 }
KCHuang 4:3b514de5e8a8 141 servo.write(servo_duty);
KCHuang 4:3b514de5e8a8 142 i=0;
KCHuang 4:3b514de5e8a8 143 }
KCHuang 4:3b514de5e8a8 144 else
KCHuang 4:3b514de5e8a8 145 {
KCHuang 4:3b514de5e8a8 146 i++;
KCHuang 4:3b514de5e8a8 147 }
YCTung 0:0971f0666990 148 }
cpul5338 3:0856a9fa97ec 149 //****************************************************************************** End of timer_interrupt
YCTung 0:0971f0666990 150
cpul5338 3:0856a9fa97ec 151 //****************************************************************************** CN_interrupt
cpul5338 3:0856a9fa97ec 152 void CN_interrupt()
YCTung 0:0971f0666990 153 {
cpul5338 3:0856a9fa97ec 154 // Motor1
cpul5338 3:0856a9fa97ec 155 // Read the current status of hall sensor
cpul5338 3:0856a9fa97ec 156 HallA_state_1 = HallA_1.read();
cpul5338 3:0856a9fa97ec 157 HallB_state_1 = HallB_1.read();
cpul5338 3:0856a9fa97ec 158
cpul5338 3:0856a9fa97ec 159 ///code for state determination///
KCHuang 4:3b514de5e8a8 160 if(HallA_state_1 == 0 && HallB_state_1 == 0)
KCHuang 4:3b514de5e8a8 161 motor_state_1 = 1;
KCHuang 4:3b514de5e8a8 162 else if(HallA_state_1 == 0 && HallB_state_1 == 1)
KCHuang 4:3b514de5e8a8 163 motor_state_1 = 2;
KCHuang 4:3b514de5e8a8 164 else if(HallA_state_1 == 1 && HallB_state_1 == 1)
KCHuang 4:3b514de5e8a8 165 motor_state_1 = 3;
KCHuang 4:3b514de5e8a8 166 else if(HallA_state_1 == 1 && HallB_state_1 == 0)
KCHuang 4:3b514de5e8a8 167 motor_state_1 = 4;
YCTung 0:0971f0666990 168
KCHuang 4:3b514de5e8a8 169 if(motor_state_old_1 != 0)
KCHuang 4:3b514de5e8a8 170 {
KCHuang 4:3b514de5e8a8 171 if(motor_state_old_1 < motor_state_1)
KCHuang 4:3b514de5e8a8 172 count_1 += 1;
KCHuang 4:3b514de5e8a8 173 else if(motor_state_old_1 > motor_state_1)
KCHuang 4:3b514de5e8a8 174 count_1 -= 1;
KCHuang 4:3b514de5e8a8 175 if(motor_state_old_1 == 4 && motor_state_1 == 1)
KCHuang 4:3b514de5e8a8 176 count_1 += 2;
KCHuang 4:3b514de5e8a8 177 if(motor_state_old_1 == 1 && motor_state_1 == 4)
KCHuang 4:3b514de5e8a8 178 count_1 -= 2;
KCHuang 4:3b514de5e8a8 179 }
KCHuang 4:3b514de5e8a8 180 motor_state_old_1 = motor_state_1;
YCTung 0:0971f0666990 181 //////////////////////////////////
YCTung 0:0971f0666990 182
YCTung 0:0971f0666990 183 //Forward
YCTung 0:0971f0666990 184 //v1Count +1
YCTung 0:0971f0666990 185 //Inverse
YCTung 0:0971f0666990 186 //v1Count -1
cpul5338 3:0856a9fa97ec 187
cpul5338 3:0856a9fa97ec 188 // Motor2
cpul5338 3:0856a9fa97ec 189 // Read the current status of hall sensor
cpul5338 3:0856a9fa97ec 190 HallA_state_2 = HallA_2.read();
cpul5338 3:0856a9fa97ec 191 HallB_state_2 = HallB_2.read();
KCHuang 4:3b514de5e8a8 192
YCTung 0:0971f0666990 193 ///code for state determination///
KCHuang 4:3b514de5e8a8 194 if(HallA_state_2 == 0 && HallB_state_2 == 0)
KCHuang 4:3b514de5e8a8 195 motor_state_2 = 1;
KCHuang 4:3b514de5e8a8 196 else if(HallA_state_2 == 0 && HallB_state_2 == 1)
KCHuang 4:3b514de5e8a8 197 motor_state_2 = 2;
KCHuang 4:3b514de5e8a8 198 else if(HallA_state_2 == 1 && HallB_state_2 == 1)
KCHuang 4:3b514de5e8a8 199 motor_state_2 = 3;
KCHuang 4:3b514de5e8a8 200 else if(HallA_state_2 == 1 && HallB_state_2 == 0)
KCHuang 4:3b514de5e8a8 201 motor_state_2 = 4;
YCTung 0:0971f0666990 202
KCHuang 4:3b514de5e8a8 203 if(motor_state_old_2 != 0)
KCHuang 4:3b514de5e8a8 204 {
KCHuang 4:3b514de5e8a8 205 if(motor_state_old_2 < motor_state_2)
KCHuang 4:3b514de5e8a8 206 count_2 += 1;
KCHuang 4:3b514de5e8a8 207 else if(motor_state_old_2 > motor_state_2)
KCHuang 4:3b514de5e8a8 208 count_2 -= 1;
KCHuang 4:3b514de5e8a8 209 if(motor_state_old_2 == 4 && motor_state_2 == 1)
KCHuang 4:3b514de5e8a8 210 count_2 += 2;
KCHuang 4:3b514de5e8a8 211 if(motor_state_old_2 == 1 && motor_state_2 == 4)
KCHuang 4:3b514de5e8a8 212 count_2 -= 2;
KCHuang 4:3b514de5e8a8 213 }
KCHuang 4:3b514de5e8a8 214 motor_state_old_2 = motor_state_2;
YCTung 0:0971f0666990 215
YCTung 0:0971f0666990 216 //////////////////////////////////
YCTung 0:0971f0666990 217
YCTung 0:0971f0666990 218 //Forward
YCTung 0:0971f0666990 219 //v2Count +1
YCTung 0:0971f0666990 220 //Inverse
YCTung 0:0971f0666990 221 //v2Count -1
YCTung 0:0971f0666990 222 }
cpul5338 3:0856a9fa97ec 223 //****************************************************************************** End of CN_interrupt
YCTung 0:0971f0666990 224
cpul5338 3:0856a9fa97ec 225 //****************************************************************************** init_timer
cpul5338 3:0856a9fa97ec 226 void init_timer()
YCTung 0:0971f0666990 227 {
cpul5338 3:0856a9fa97ec 228 timer.attach_us(&timer_interrupt, 10000);//10ms interrupt period (100 Hz)
cpul5338 3:0856a9fa97ec 229 }
cpul5338 3:0856a9fa97ec 230 //****************************************************************************** End of init_timer
cpul5338 3:0856a9fa97ec 231
cpul5338 3:0856a9fa97ec 232 //****************************************************************************** init_PWM
cpul5338 3:0856a9fa97ec 233 void init_PWM()
YCTung 0:0971f0666990 234 {
YCTung 0:0971f0666990 235 pwm1.period_us(50);
YCTung 0:0971f0666990 236 pwm1.write(0.5);
YCTung 0:0971f0666990 237 TIM1->CCER |= 0x4;
YCTung 0:0971f0666990 238
YCTung 0:0971f0666990 239 pwm2.period_us(50);
YCTung 0:0971f0666990 240 pwm2.write(0.5);
YCTung 0:0971f0666990 241 TIM1->CCER |= 0x40;
KCHuang 4:3b514de5e8a8 242
KCHuang 4:3b514de5e8a8 243 servo.period_ms(Servo_Period);
KCHuang 4:3b514de5e8a8 244 servo.write(servo_duty);
YCTung 0:0971f0666990 245 }
cpul5338 3:0856a9fa97ec 246 //****************************************************************************** End of init_PWM
YCTung 0:0971f0666990 247
cpul5338 3:0856a9fa97ec 248 //****************************************************************************** init_CN
cpul5338 3:0856a9fa97ec 249 void init_CN()
YCTung 0:0971f0666990 250 {
cpul5338 3:0856a9fa97ec 251 // Motor1
YCTung 0:0971f0666990 252 HallA_1.rise(&CN_interrupt);
YCTung 0:0971f0666990 253 HallA_1.fall(&CN_interrupt);
YCTung 0:0971f0666990 254 HallB_1.rise(&CN_interrupt);
YCTung 0:0971f0666990 255 HallB_1.fall(&CN_interrupt);
YCTung 0:0971f0666990 256
cpul5338 3:0856a9fa97ec 257 HallA_state_1 = HallA_1.read();
cpul5338 3:0856a9fa97ec 258 HallB_state_1 = HallB_1.read();
cpul5338 3:0856a9fa97ec 259
cpul5338 3:0856a9fa97ec 260 // Motor2
YCTung 0:0971f0666990 261 HallA_2.rise(&CN_interrupt);
YCTung 0:0971f0666990 262 HallA_2.fall(&CN_interrupt);
YCTung 0:0971f0666990 263 HallB_2.rise(&CN_interrupt);
YCTung 0:0971f0666990 264 HallB_2.fall(&CN_interrupt);
YCTung 0:0971f0666990 265
cpul5338 3:0856a9fa97ec 266 HallA_state_2 = HallA_2.read();
cpul5338 3:0856a9fa97ec 267 HallB_state_2 = HallB_2.read();
roger5641 2:9caf46a5fe5a 268 }
cpul5338 3:0856a9fa97ec 269 //****************************************************************************** End of init_CN