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?
User | Revision | Line number | New 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 | } |