Roger Weng / Mbed 2 deprecated Encoder

work

Dependencies:   LSM9DS0 PID QEI mbed

main.cpp@0:cb8ba5eb9f7e, 2016-09-23 (annotated)

Committer:
roger5641
Date:
Fri Sep 23 09:38:27 2016 +0000
Revision:
0:cb8ba5eb9f7e
work

Who changed what in which revision?

UserRevisionLine numberNew contents of line
roger5641 0:cb8ba5eb9f7e 1 #include "mbed.h"
roger5641 0:cb8ba5eb9f7e 2 #include "QEI.h"
roger5641 0:cb8ba5eb9f7e 3 #include "PID.h"
roger5641 0:cb8ba5eb9f7e 4
roger5641 0:cb8ba5eb9f7e 5
roger5641 0:cb8ba5eb9f7e 6 Serial bluetooth(D10, D2); //uart for bluretooth
roger5641 0:cb8ba5eb9f7e 7 Serial pc(D1, D0); //uart for pc
roger5641 0:cb8ba5eb9f7e 8
roger5641 0:cb8ba5eb9f7e 9 QEI wheel_L (A1, A2, NC, 1560, 10, 0.01, QEI::X4_ENCODING); //390*4 = 1560 //chang array 50 to 10
roger5641 0:cb8ba5eb9f7e 10 QEI wheel_R (D13, D12, NC, 1560, 10, 0.01, QEI::X4_ENCODING);//390*4 = 1560 //chang array 50 to 10
roger5641 0:cb8ba5eb9f7e 11 PID speed_L_PID(0.1, 0.1, 0.0, 0.01);//0.00001, 0.00001, 0,,,//0.08, 0.08, 0.01, 0.01, 0.02
roger5641 0:cb8ba5eb9f7e 12 PID speed_R_PID(0.07, 0.07, 0.0, 0.01);//(0.1, 0.000005, 0, 0.01 //0.22, 0.11, 0.005, 0.01
roger5641 0:cb8ba5eb9f7e 13
roger5641 0:cb8ba5eb9f7e 14 PwmOut motor_L_pwm(D7);
roger5641 0:cb8ba5eb9f7e 15 PwmOut motor_L_pwmn(D11);
roger5641 0:cb8ba5eb9f7e 16 PwmOut motor_R_pwm(D8);
roger5641 0:cb8ba5eb9f7e 17 PwmOut motor_R_pwmn(A3);
roger5641 0:cb8ba5eb9f7e 18
roger5641 0:cb8ba5eb9f7e 19 Ticker timer1;
roger5641 0:cb8ba5eb9f7e 20 void timer1_interrupt(void);
roger5641 0:cb8ba5eb9f7e 21 void init_PWM(void);
roger5641 0:cb8ba5eb9f7e 22 void init_TIMER(void);
roger5641 0:cb8ba5eb9f7e 23 void init_sensor(void);
roger5641 0:cb8ba5eb9f7e 24 void real_sensor_value(void);
roger5641 0:cb8ba5eb9f7e 25 void sensor_fusion(void);
roger5641 0:cb8ba5eb9f7e 26 float lpf(float input, float output_old, float frequency);
roger5641 0:cb8ba5eb9f7e 27
roger5641 0:cb8ba5eb9f7e 28 float motor_L_AngularSpeed;
roger5641 0:cb8ba5eb9f7e 29 float motor_R_AngularSpeed;
roger5641 0:cb8ba5eb9f7e 30 float sample_speed = 0.0;
roger5641 0:cb8ba5eb9f7e 31 float pwm1_duty = 0.5f;
roger5641 0:cb8ba5eb9f7e 32 float pwm2_duty = 0.5f;
roger5641 0:cb8ba5eb9f7e 33
roger5641 0:cb8ba5eb9f7e 34 char Idle_Mode = 'a';
roger5641 0:cb8ba5eb9f7e 35 char robot_Mode = 'a';
roger5641 0:cb8ba5eb9f7e 36 char speed_2 = 'b';
roger5641 0:cb8ba5eb9f7e 37 char speed_3 = 'c';
roger5641 0:cb8ba5eb9f7e 38 char speed_4 = 'd';
roger5641 0:cb8ba5eb9f7e 39 char speed_5 = 'e';
roger5641 0:cb8ba5eb9f7e 40 char speed_6 = 'f';
roger5641 0:cb8ba5eb9f7e 41 char speed_7 = 'g';
roger5641 0:cb8ba5eb9f7e 42 char speed_8 = 'h';
roger5641 0:cb8ba5eb9f7e 43
roger5641 0:cb8ba5eb9f7e 44 int main() {
roger5641 0:cb8ba5eb9f7e 45
roger5641 0:cb8ba5eb9f7e 46 pc.baud(115200);
roger5641 0:cb8ba5eb9f7e 47 bluetooth.baud(115200);
roger5641 0:cb8ba5eb9f7e 48 init_PWM();
roger5641 0:cb8ba5eb9f7e 49 init_TIMER();
roger5641 0:cb8ba5eb9f7e 50
roger5641 0:cb8ba5eb9f7e 51 while(1)
roger5641 0:cb8ba5eb9f7e 52 {
roger5641 0:cb8ba5eb9f7e 53 ;
roger5641 0:cb8ba5eb9f7e 54 }
roger5641 0:cb8ba5eb9f7e 55 }
roger5641 0:cb8ba5eb9f7e 56
roger5641 0:cb8ba5eb9f7e 57 void timer1_interrupt(void)
roger5641 0:cb8ba5eb9f7e 58 {
roger5641 0:cb8ba5eb9f7e 59 wheel_L.Calculate();
roger5641 0:cb8ba5eb9f7e 60 wheel_R.Calculate();
roger5641 0:cb8ba5eb9f7e 61
roger5641 0:cb8ba5eb9f7e 62 if(bluetooth.readable())
roger5641 0:cb8ba5eb9f7e 63 {
roger5641 0:cb8ba5eb9f7e 64 robot_Mode = bluetooth.getc();
roger5641 0:cb8ba5eb9f7e 65 }
roger5641 0:cb8ba5eb9f7e 66
roger5641 0:cb8ba5eb9f7e 67 if(robot_Mode == Idle_Mode)
roger5641 0:cb8ba5eb9f7e 68 {
roger5641 0:cb8ba5eb9f7e 69 //bluetooth.printf("Idle_Mode\n");
roger5641 0:cb8ba5eb9f7e 70 pwm1_duty = 0.5f;
roger5641 0:cb8ba5eb9f7e 71 pwm2_duty = 0.5f;
roger5641 0:cb8ba5eb9f7e 72
roger5641 0:cb8ba5eb9f7e 73 }
roger5641 0:cb8ba5eb9f7e 74 else if(robot_Mode == speed_2)
roger5641 0:cb8ba5eb9f7e 75 {
roger5641 0:cb8ba5eb9f7e 76 //bluetooth.printf("speed_2\n");
roger5641 0:cb8ba5eb9f7e 77
roger5641 0:cb8ba5eb9f7e 78 sample_speed = 2.0;
roger5641 0:cb8ba5eb9f7e 79
roger5641 0:cb8ba5eb9f7e 80 speed_L_PID.Compute(-sample_speed, wheel_L.getAngularSpeed());
roger5641 0:cb8ba5eb9f7e 81 speed_R_PID.Compute(sample_speed, wheel_R.getAngularSpeed());
roger5641 0:cb8ba5eb9f7e 82
roger5641 0:cb8ba5eb9f7e 83 pwm1_duty = 0.5f + speed_L_PID.output;
roger5641 0:cb8ba5eb9f7e 84 pwm2_duty = 0.5f + speed_R_PID.output;
roger5641 0:cb8ba5eb9f7e 85
roger5641 0:cb8ba5eb9f7e 86 }
roger5641 0:cb8ba5eb9f7e 87 else if(robot_Mode == speed_3)
roger5641 0:cb8ba5eb9f7e 88 {
roger5641 0:cb8ba5eb9f7e 89 //bluetooth.printf("speed_3\n");
roger5641 0:cb8ba5eb9f7e 90
roger5641 0:cb8ba5eb9f7e 91 sample_speed = 3.0;
roger5641 0:cb8ba5eb9f7e 92
roger5641 0:cb8ba5eb9f7e 93 speed_L_PID.Compute(sample_speed, wheel_L.getAngularSpeed());
roger5641 0:cb8ba5eb9f7e 94 speed_R_PID.Compute(-sample_speed, wheel_R.getAngularSpeed());
roger5641 0:cb8ba5eb9f7e 95
roger5641 0:cb8ba5eb9f7e 96 pwm1_duty = 0.5f + speed_L_PID.output;
roger5641 0:cb8ba5eb9f7e 97 pwm2_duty = 0.5f + speed_R_PID.output;
roger5641 0:cb8ba5eb9f7e 98
roger5641 0:cb8ba5eb9f7e 99 }
roger5641 0:cb8ba5eb9f7e 100 else if(robot_Mode == speed_4)
roger5641 0:cb8ba5eb9f7e 101 {
roger5641 0:cb8ba5eb9f7e 102 //bluetooth.printf("speed_4\n");
roger5641 0:cb8ba5eb9f7e 103
roger5641 0:cb8ba5eb9f7e 104 sample_speed = 4.0;
roger5641 0:cb8ba5eb9f7e 105
roger5641 0:cb8ba5eb9f7e 106 speed_L_PID.Compute(-sample_speed, wheel_L.getAngularSpeed());
roger5641 0:cb8ba5eb9f7e 107 speed_R_PID.Compute(sample_speed, wheel_R.getAngularSpeed());
roger5641 0:cb8ba5eb9f7e 108
roger5641 0:cb8ba5eb9f7e 109 pwm1_duty = 0.5f + speed_L_PID.output;
roger5641 0:cb8ba5eb9f7e 110 pwm2_duty = 0.5f + speed_R_PID.output;
roger5641 0:cb8ba5eb9f7e 111 }
roger5641 0:cb8ba5eb9f7e 112 else if(robot_Mode == speed_5)
roger5641 0:cb8ba5eb9f7e 113 {
roger5641 0:cb8ba5eb9f7e 114 //bluetooth.printf("speed_5\n");
roger5641 0:cb8ba5eb9f7e 115
roger5641 0:cb8ba5eb9f7e 116 sample_speed = 5.0;
roger5641 0:cb8ba5eb9f7e 117
roger5641 0:cb8ba5eb9f7e 118 speed_L_PID.Compute(sample_speed, wheel_L.getAngularSpeed());
roger5641 0:cb8ba5eb9f7e 119 speed_R_PID.Compute(-sample_speed, wheel_R.getAngularSpeed());
roger5641 0:cb8ba5eb9f7e 120
roger5641 0:cb8ba5eb9f7e 121 pwm1_duty = 0.5f + speed_L_PID.output;
roger5641 0:cb8ba5eb9f7e 122 pwm2_duty = 0.5f + speed_R_PID.output;
roger5641 0:cb8ba5eb9f7e 123 }
roger5641 0:cb8ba5eb9f7e 124 else if(robot_Mode == speed_6)
roger5641 0:cb8ba5eb9f7e 125 {
roger5641 0:cb8ba5eb9f7e 126 //bluetooth.printf("speed_6\n");
roger5641 0:cb8ba5eb9f7e 127
roger5641 0:cb8ba5eb9f7e 128 sample_speed = 6.0;
roger5641 0:cb8ba5eb9f7e 129
roger5641 0:cb8ba5eb9f7e 130 speed_L_PID.Compute(-sample_speed, wheel_L.getAngularSpeed());
roger5641 0:cb8ba5eb9f7e 131 speed_R_PID.Compute(sample_speed, wheel_R.getAngularSpeed());
roger5641 0:cb8ba5eb9f7e 132
roger5641 0:cb8ba5eb9f7e 133 pwm1_duty = 0.5f + speed_L_PID.output;
roger5641 0:cb8ba5eb9f7e 134 pwm2_duty = 0.5f + speed_R_PID.output;
roger5641 0:cb8ba5eb9f7e 135
roger5641 0:cb8ba5eb9f7e 136 }
roger5641 0:cb8ba5eb9f7e 137 else if(robot_Mode == speed_7)
roger5641 0:cb8ba5eb9f7e 138 {
roger5641 0:cb8ba5eb9f7e 139 //bluetooth.printf("speed_7\n");
roger5641 0:cb8ba5eb9f7e 140
roger5641 0:cb8ba5eb9f7e 141 sample_speed = 7.0;
roger5641 0:cb8ba5eb9f7e 142
roger5641 0:cb8ba5eb9f7e 143 speed_L_PID.Compute(sample_speed, wheel_L.getAngularSpeed());
roger5641 0:cb8ba5eb9f7e 144 speed_R_PID.Compute(-sample_speed, wheel_R.getAngularSpeed());
roger5641 0:cb8ba5eb9f7e 145
roger5641 0:cb8ba5eb9f7e 146 pwm1_duty = 0.5f + speed_L_PID.output;
roger5641 0:cb8ba5eb9f7e 147 pwm2_duty = 0.5f + speed_R_PID.output;
roger5641 0:cb8ba5eb9f7e 148
roger5641 0:cb8ba5eb9f7e 149 }
roger5641 0:cb8ba5eb9f7e 150 else if(robot_Mode == speed_8)
roger5641 0:cb8ba5eb9f7e 151 {
roger5641 0:cb8ba5eb9f7e 152 //bluetooth.printf("speed_8\n");
roger5641 0:cb8ba5eb9f7e 153
roger5641 0:cb8ba5eb9f7e 154 sample_speed = 8.0;
roger5641 0:cb8ba5eb9f7e 155
roger5641 0:cb8ba5eb9f7e 156 speed_L_PID.Compute(-sample_speed, wheel_L.getAngularSpeed());
roger5641 0:cb8ba5eb9f7e 157 speed_R_PID.Compute(sample_speed, wheel_R.getAngularSpeed());
roger5641 0:cb8ba5eb9f7e 158
roger5641 0:cb8ba5eb9f7e 159 pwm1_duty = 0.5f + speed_L_PID.output;
roger5641 0:cb8ba5eb9f7e 160 pwm2_duty = 0.5f + speed_R_PID.output;
roger5641 0:cb8ba5eb9f7e 161
roger5641 0:cb8ba5eb9f7e 162 }
roger5641 0:cb8ba5eb9f7e 163 else
roger5641 0:cb8ba5eb9f7e 164 {
roger5641 0:cb8ba5eb9f7e 165 pwm1_duty = 0.5f;
roger5641 0:cb8ba5eb9f7e 166 pwm2_duty = 0.5f;
roger5641 0:cb8ba5eb9f7e 167 }
roger5641 0:cb8ba5eb9f7e 168
roger5641 0:cb8ba5eb9f7e 169 motor_L_pwm.write(pwm1_duty); //>0.5 forward(-) ; <0.5 backward(+)
roger5641 0:cb8ba5eb9f7e 170 motor_R_pwm.write(pwm2_duty); // <0.5 forward(-) ; >0.5 backward(+)
roger5641 0:cb8ba5eb9f7e 171
roger5641 0:cb8ba5eb9f7e 172 TIM1->CCER |= 4; //enable ch1 complimentary output
roger5641 0:cb8ba5eb9f7e 173 TIM1->CCER |= 64; //enable ch2 complimentary output
roger5641 0:cb8ba5eb9f7e 174
roger5641 0:cb8ba5eb9f7e 175
roger5641 0:cb8ba5eb9f7e 176 pc.printf("motor_L_AngularSpeed: %f motor_R_AngularSpeed: %f \r\n",wheel_L.getAngularSpeed(), wheel_R.getAngularSpeed());
roger5641 0:cb8ba5eb9f7e 177 }
roger5641 0:cb8ba5eb9f7e 178
roger5641 0:cb8ba5eb9f7e 179 void init_TIMER(void)
roger5641 0:cb8ba5eb9f7e 180 {
roger5641 0:cb8ba5eb9f7e 181 timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
roger5641 0:cb8ba5eb9f7e 182 }
roger5641 0:cb8ba5eb9f7e 183
roger5641 0:cb8ba5eb9f7e 184 void init_PWM(void)
roger5641 0:cb8ba5eb9f7e 185 {
roger5641 0:cb8ba5eb9f7e 186 motor_L_pwm.period_us(50);
roger5641 0:cb8ba5eb9f7e 187 motor_L_pwm.write(0.5f);
roger5641 0:cb8ba5eb9f7e 188
roger5641 0:cb8ba5eb9f7e 189 motor_R_pwm.period_us(50);
roger5641 0:cb8ba5eb9f7e 190 motor_R_pwm.write(0.5f);
roger5641 0:cb8ba5eb9f7e 191
roger5641 0:cb8ba5eb9f7e 192 TIM1->CCER |= 4;
roger5641 0:cb8ba5eb9f7e 193 TIM1->CCER |= 64;
roger5641 0:cb8ba5eb9f7e 194
roger5641 0:cb8ba5eb9f7e 195 speed_L_PID.SetOutputLimits(0.5, -0.5);
roger5641 0:cb8ba5eb9f7e 196 speed_R_PID.SetOutputLimits(0.5, -0.5);
roger5641 0:cb8ba5eb9f7e 197 }