mobile_robot_tea / Mbed 2 deprecated motor_control

lab2

Dependencies:   mbed hallsensor_software_decoder

main.cpp@0:b124e4e3a1e4, 2019-03-13 (annotated)

Committer:
bobolee1239
Date:
Wed Mar 13 06:26:51 2019 +0000
Revision:
0:b124e4e3a1e4
Child:
1:47a1aaf98baa
fdafdsafdsaf dsf; ; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bobolee1239 0:b124e4e3a1e4 1 #include "mbed.h"
bobolee1239 0:b124e4e3a1e4 2 #include "hallsensor_software_decoder.h"
bobolee1239 0:b124e4e3a1e4 3
bobolee1239 0:b124e4e3a1e4 4 #define Ts 0.01f // f to specify float type
bobolee1239 0:b124e4e3a1e4 5
bobolee1239 0:b124e4e3a1e4 6 Ticker timer1;
bobolee1239 0:b124e4e3a1e4 7
bobolee1239 0:b124e4e3a1e4 8 PwmOut pwm1(D7);
bobolee1239 0:b124e4e3a1e4 9 PwmOut pwm1n(D11);
bobolee1239 0:b124e4e3a1e4 10 PwmOut pwm2(D8);
bobolee1239 0:b124e4e3a1e4 11 PwmOut pwm2n(A3);
bobolee1239 0:b124e4e3a1e4 12
bobolee1239 0:b124e4e3a1e4 13 void init_TIMER();
bobolee1239 0:b124e4e3a1e4 14 void init_PWM();
bobolee1239 0:b124e4e3a1e4 15 void timer1_ITR();
bobolee1239 0:b124e4e3a1e4 16
bobolee1239 0:b124e4e3a1e4 17 /* wheel 1 */
bobolee1239 0:b124e4e3a1e4 18 float rotation_speed_1 = 0.0;
bobolee1239 0:b124e4e3a1e4 19 float pwm1_duty = 0.5;
bobolee1239 0:b124e4e3a1e4 20 double PI_out_1 = 0.0;
bobolee1239 0:b124e4e3a1e4 21 float err_1 = 0.0;
bobolee1239 0:b124e4e3a1e4 22 float ierr_1 = 0.0;
bobolee1239 0:b124e4e3a1e4 23
bobolee1239 0:b124e4e3a1e4 24 /* wheel 2 */
bobolee1239 0:b124e4e3a1e4 25 float rotation_speed_2 = 0.0;
bobolee1239 0:b124e4e3a1e4 26 float pwm2_duty = 0.5;
bobolee1239 0:b124e4e3a1e4 27 float PI_out_2 = 0.0;
bobolee1239 0:b124e4e3a1e4 28 float err_2 = 0.0;
bobolee1239 0:b124e4e3a1e4 29 float ierr_2 = 0.0;
bobolee1239 0:b124e4e3a1e4 30
bobolee1239 0:b124e4e3a1e4 31 float r = 0.03; // wheel radius (m)
bobolee1239 0:b124e4e3a1e4 32 float L = 0.27; // car width (m)
bobolee1239 0:b124e4e3a1e4 33
bobolee1239 0:b124e4e3a1e4 34 float rotation_speed_ref_1 = 25.0;
bobolee1239 0:b124e4e3a1e4 35 float rotation_speed_ref_2 = -25.0;
bobolee1239 0:b124e4e3a1e4 36
bobolee1239 0:b124e4e3a1e4 37 float V_ref = 0.0; // (m/s) max: 0.25
bobolee1239 0:b124e4e3a1e4 38 float V = 0.0;
bobolee1239 0:b124e4e3a1e4 39 float W_ref = 0.0; // (rad/s)
bobolee1239 0:b124e4e3a1e4 40 float W = 0.0;
bobolee1239 0:b124e4e3a1e4 41
bobolee1239 0:b124e4e3a1e4 42
bobolee1239 0:b124e4e3a1e4 43 float Kp = 0.008;
bobolee1239 0:b124e4e3a1e4 44 float Ki = 0.02;
bobolee1239 0:b124e4e3a1e4 45
bobolee1239 0:b124e4e3a1e4 46 //Serial pc(USBTX, USBRX);
bobolee1239 0:b124e4e3a1e4 47
bobolee1239 0:b124e4e3a1e4 48
bobolee1239 0:b124e4e3a1e4 49 int main()
bobolee1239 0:b124e4e3a1e4 50 {
bobolee1239 0:b124e4e3a1e4 51 //pc.baud(9600);
bobolee1239 0:b124e4e3a1e4 52 init_TIMER();
bobolee1239 0:b124e4e3a1e4 53 init_PWM();
bobolee1239 0:b124e4e3a1e4 54 init_CN();
bobolee1239 0:b124e4e3a1e4 55
bobolee1239 0:b124e4e3a1e4 56 while(1) {
bobolee1239 0:b124e4e3a1e4 57 V = ((rotation_speed_1 + rotation_speed_2) * r / 2.0f )* 2.0f * 3.14f / 60.0f;
bobolee1239 0:b124e4e3a1e4 58 W = (-2.0f * r * (rotation_speed_1 - rotation_speed_2) / L) * 2.0f * 3.14f / 60.0f;
bobolee1239 0:b124e4e3a1e4 59 /* pc.printf("motor_1 = %f, motor_2 = %f\r\n", rotation_speed_1,rotation_speed_2);
bobolee1239 0:b124e4e3a1e4 60 pc.printf("V = %f, W = %f\r\n", V,W);*/
bobolee1239 0:b124e4e3a1e4 61
bobolee1239 0:b124e4e3a1e4 62 wait_ms(100);
bobolee1239 0:b124e4e3a1e4 63 }
bobolee1239 0:b124e4e3a1e4 64 }
bobolee1239 0:b124e4e3a1e4 65
bobolee1239 0:b124e4e3a1e4 66
bobolee1239 0:b124e4e3a1e4 67
bobolee1239 0:b124e4e3a1e4 68 void init_TIMER() {
bobolee1239 0:b124e4e3a1e4 69 /********************************************************
bobolee1239 0:b124e4e3a1e4 70 * function to be attached (timer1_ITR) and
bobolee1239 0:b124e4e3a1e4 71 * the interval (10000 micro-seconds) = 0.01 second
bobolee1239 0:b124e4e3a1e4 72 *********************************************************/
bobolee1239 0:b124e4e3a1e4 73 timer1.attach_us(&timer1_ITR, 10000.0);
bobolee1239 0:b124e4e3a1e4 74 }
bobolee1239 0:b124e4e3a1e4 75
bobolee1239 0:b124e4e3a1e4 76
bobolee1239 0:b124e4e3a1e4 77 void init_PWM(){
bobolee1239 0:b124e4e3a1e4 78 /* setting pwm width */
bobolee1239 0:b124e4e3a1e4 79
bobolee1239 0:b124e4e3a1e4 80 pwm1.period_us(50);
bobolee1239 0:b124e4e3a1e4 81 pwm1.write(0.5);
bobolee1239 0:b124e4e3a1e4 82 TIM1->CCER |= 0x4;
bobolee1239 0:b124e4e3a1e4 83
bobolee1239 0:b124e4e3a1e4 84 pwm2.period_us(50);
bobolee1239 0:b124e4e3a1e4 85 pwm2.write(0.5);
bobolee1239 0:b124e4e3a1e4 86 TIM1->CCER |= 0x40;
bobolee1239 0:b124e4e3a1e4 87 }
bobolee1239 0:b124e4e3a1e4 88
bobolee1239 0:b124e4e3a1e4 89
bobolee1239 0:b124e4e3a1e4 90 void timer1_ITR()
bobolee1239 0:b124e4e3a1e4 91 {
bobolee1239 0:b124e4e3a1e4 92
bobolee1239 0:b124e4e3a1e4 93 //rotation_speed_ref_1 = (V_ref / r - L * W_ref / 4.0f / r) / 2.0f / 3.14f * 60.0f;
bobolee1239 0:b124e4e3a1e4 94 //rotation_speed_ref_2 = (V_ref / r + L * W_ref / 4.0f / r) / 2.0f / 3.14f * 60.0f;
bobolee1239 0:b124e4e3a1e4 95
bobolee1239 0:b124e4e3a1e4 96
bobolee1239 0:b124e4e3a1e4 97 /********** MOTOR1 **********/
bobolee1239 0:b124e4e3a1e4 98
bobolee1239 0:b124e4e3a1e4 99 //measure
bobolee1239 0:b124e4e3a1e4 100 rotation_speed_1 = (float)speed_count_1 * 100.0f / 48.0f * 60.0f / 56.0f; //unit: rpm
bobolee1239 0:b124e4e3a1e4 101 // reset
bobolee1239 0:b124e4e3a1e4 102 speed_count_1 = 0;
bobolee1239 0:b124e4e3a1e4 103
bobolee1239 0:b124e4e3a1e4 104 /************ PI controller for motor1 *************/
bobolee1239 0:b124e4e3a1e4 105
bobolee1239 0:b124e4e3a1e4 106 err_1 = rotation_speed_ref_1 - rotation_speed_1;
bobolee1239 0:b124e4e3a1e4 107 ierr_1 = ierr_1 + Ts * err_1;
bobolee1239 0:b124e4e3a1e4 108
bobolee1239 0:b124e4e3a1e4 109 // restrict integrating part
bobolee1239 0:b124e4e3a1e4 110 if(ierr_1 > 50.0f){
bobolee1239 0:b124e4e3a1e4 111 ierr_1 = 50.0;
bobolee1239 0:b124e4e3a1e4 112 } else if(ierr_1 < -50.0f){
bobolee1239 0:b124e4e3a1e4 113 ierr_1 = -50.0;
bobolee1239 0:b124e4e3a1e4 114 }
bobolee1239 0:b124e4e3a1e4 115
bobolee1239 0:b124e4e3a1e4 116 PI_out_1 = (Kp * err_1 + Ki * ierr_1);
bobolee1239 0:b124e4e3a1e4 117
bobolee1239 0:b124e4e3a1e4 118 // staturation
bobolee1239 0:b124e4e3a1e4 119 if(PI_out_1 >= 0.5f){
bobolee1239 0:b124e4e3a1e4 120 PI_out_1 = 0.5;
bobolee1239 0:b124e4e3a1e4 121 } else if(PI_out_1 <= -0.5f){
bobolee1239 0:b124e4e3a1e4 122 PI_out_1 = -0.5;
bobolee1239 0:b124e4e3a1e4 123 }
bobolee1239 0:b124e4e3a1e4 124 /*************************************************/
bobolee1239 0:b124e4e3a1e4 125
bobolee1239 0:b124e4e3a1e4 126 /* output to acuator */
bobolee1239 0:b124e4e3a1e4 127 pwm1_duty = PI_out_1 + 0.5f;
bobolee1239 0:b124e4e3a1e4 128 pwm1.write(pwm1_duty);
bobolee1239 0:b124e4e3a1e4 129 TIM1->CCER |= 0x4;
bobolee1239 0:b124e4e3a1e4 130
bobolee1239 0:b124e4e3a1e4 131
bobolee1239 0:b124e4e3a1e4 132
bobolee1239 0:b124e4e3a1e4 133
bobolee1239 0:b124e4e3a1e4 134 /********* MOTOR2 ***********/
bobolee1239 0:b124e4e3a1e4 135
bobolee1239 0:b124e4e3a1e4 136
bobolee1239 0:b124e4e3a1e4 137 rotation_speed_2 = (float)speed_count_2 * 100.0f / 48.0f * 60.0f / 56.0f; //unit: rpm
bobolee1239 0:b124e4e3a1e4 138 speed_count_2 = 0;
bobolee1239 0:b124e4e3a1e4 139
bobolee1239 0:b124e4e3a1e4 140 /************* PI controller for motor2 ****************/
bobolee1239 0:b124e4e3a1e4 141 err_2 = rotation_speed_ref_2 - rotation_speed_2;
bobolee1239 0:b124e4e3a1e4 142 ierr_2 = ierr_2 + Ts * err_2;
bobolee1239 0:b124e4e3a1e4 143
bobolee1239 0:b124e4e3a1e4 144 // restrict integration output
bobolee1239 0:b124e4e3a1e4 145 if(ierr_2 > 50.0f){
bobolee1239 0:b124e4e3a1e4 146 ierr_2 = 50.0;
bobolee1239 0:b124e4e3a1e4 147 } else if(ierr_2 < -50.0f){
bobolee1239 0:b124e4e3a1e4 148 ierr_2 = -50.0;
bobolee1239 0:b124e4e3a1e4 149 }
bobolee1239 0:b124e4e3a1e4 150 PI_out_2 = -(Kp * err_2 + Ki * ierr_2) ;
bobolee1239 0:b124e4e3a1e4 151
bobolee1239 0:b124e4e3a1e4 152 // staturation
bobolee1239 0:b124e4e3a1e4 153 if(PI_out_2 >= 0.5f){
bobolee1239 0:b124e4e3a1e4 154 PI_out_2 = 0.5;
bobolee1239 0:b124e4e3a1e4 155 } else if(PI_out_2 <= -0.5f) {
bobolee1239 0:b124e4e3a1e4 156 PI_out_2 = -0.5;
bobolee1239 0:b124e4e3a1e4 157 }
bobolee1239 0:b124e4e3a1e4 158 /******************************************************/
bobolee1239 0:b124e4e3a1e4 159 /* output to acuator */
bobolee1239 0:b124e4e3a1e4 160 pwm2_duty = PI_out_2 + 0.5f;
bobolee1239 0:b124e4e3a1e4 161 pwm2.write(pwm2_duty);
bobolee1239 0:b124e4e3a1e4 162 TIM1->CCER |= 0x40;
bobolee1239 0:b124e4e3a1e4 163
bobolee1239 0:b124e4e3a1e4 164
bobolee1239 0:b124e4e3a1e4 165 }