lab2
Dependencies: mbed hallsensor_software_decoder
Diff: main.cpp
- Revision:
- 0:b124e4e3a1e4
- Child:
- 1:47a1aaf98baa
diff -r 000000000000 -r b124e4e3a1e4 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Mar 13 06:26:51 2019 +0000 @@ -0,0 +1,165 @@ +#include "mbed.h" +#include "hallsensor_software_decoder.h" + +#define Ts 0.01f // f to specify float type + +Ticker timer1; + +PwmOut pwm1(D7); +PwmOut pwm1n(D11); +PwmOut pwm2(D8); +PwmOut pwm2n(A3); + +void init_TIMER(); +void init_PWM(); +void timer1_ITR(); + +/* wheel 1 */ +float rotation_speed_1 = 0.0; +float pwm1_duty = 0.5; +double PI_out_1 = 0.0; +float err_1 = 0.0; +float ierr_1 = 0.0; + +/* wheel 2 */ +float rotation_speed_2 = 0.0; +float pwm2_duty = 0.5; +float PI_out_2 = 0.0; +float err_2 = 0.0; +float ierr_2 = 0.0; + +float r = 0.03; // wheel radius (m) +float L = 0.27; // car width (m) + +float rotation_speed_ref_1 = 25.0; +float rotation_speed_ref_2 = -25.0; + +float V_ref = 0.0; // (m/s) max: 0.25 +float V = 0.0; +float W_ref = 0.0; // (rad/s) +float W = 0.0; + + +float Kp = 0.008; +float Ki = 0.02; + +//Serial pc(USBTX, USBRX); + + +int main() +{ + //pc.baud(9600); + init_TIMER(); + init_PWM(); + init_CN(); + + while(1) { + V = ((rotation_speed_1 + rotation_speed_2) * r / 2.0f )* 2.0f * 3.14f / 60.0f; + W = (-2.0f * r * (rotation_speed_1 - rotation_speed_2) / L) * 2.0f * 3.14f / 60.0f; + /* pc.printf("motor_1 = %f, motor_2 = %f\r\n", rotation_speed_1,rotation_speed_2); + pc.printf("V = %f, W = %f\r\n", V,W);*/ + + wait_ms(100); + } +} + + + +void init_TIMER() { + /******************************************************** + * function to be attached (timer1_ITR) and + * the interval (10000 micro-seconds) = 0.01 second + *********************************************************/ + timer1.attach_us(&timer1_ITR, 10000.0); +} + + +void init_PWM(){ + /* setting pwm width */ + + pwm1.period_us(50); + pwm1.write(0.5); + TIM1->CCER |= 0x4; + + pwm2.period_us(50); + pwm2.write(0.5); + TIM1->CCER |= 0x40; +} + + +void timer1_ITR() +{ + + //rotation_speed_ref_1 = (V_ref / r - L * W_ref / 4.0f / r) / 2.0f / 3.14f * 60.0f; + //rotation_speed_ref_2 = (V_ref / r + L * W_ref / 4.0f / r) / 2.0f / 3.14f * 60.0f; + + + /********** MOTOR1 **********/ + + //measure + rotation_speed_1 = (float)speed_count_1 * 100.0f / 48.0f * 60.0f / 56.0f; //unit: rpm + // reset + speed_count_1 = 0; + + /************ PI controller for motor1 *************/ + + err_1 = rotation_speed_ref_1 - rotation_speed_1; + ierr_1 = ierr_1 + Ts * err_1; + + // restrict integrating part + if(ierr_1 > 50.0f){ + ierr_1 = 50.0; + } else if(ierr_1 < -50.0f){ + ierr_1 = -50.0; + } + + PI_out_1 = (Kp * err_1 + Ki * ierr_1); + + // staturation + if(PI_out_1 >= 0.5f){ + PI_out_1 = 0.5; + } else if(PI_out_1 <= -0.5f){ + PI_out_1 = -0.5; + } + /*************************************************/ + + /* output to acuator */ + pwm1_duty = PI_out_1 + 0.5f; + pwm1.write(pwm1_duty); + TIM1->CCER |= 0x4; + + + + + /********* MOTOR2 ***********/ + + + rotation_speed_2 = (float)speed_count_2 * 100.0f / 48.0f * 60.0f / 56.0f; //unit: rpm + speed_count_2 = 0; + + /************* PI controller for motor2 ****************/ + err_2 = rotation_speed_ref_2 - rotation_speed_2; + ierr_2 = ierr_2 + Ts * err_2; + + // restrict integration output + if(ierr_2 > 50.0f){ + ierr_2 = 50.0; + } else if(ierr_2 < -50.0f){ + ierr_2 = -50.0; + } + PI_out_2 = -(Kp * err_2 + Ki * ierr_2) ; + + // staturation + if(PI_out_2 >= 0.5f){ + PI_out_2 = 0.5; + } else if(PI_out_2 <= -0.5f) { + PI_out_2 = -0.5; + } + /******************************************************/ + /* output to acuator */ + pwm2_duty = PI_out_2 + 0.5f; + pwm2.write(pwm2_duty); + TIM1->CCER |= 0x40; + + +} \ No newline at end of file