Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
main.cpp@3:3f559b3f7656, 2022-09-19 (annotated)
- Committer:
- ohdoyoel
- Date:
- Mon Sep 19 09:59:26 2022 +0000
- Revision:
- 3:3f559b3f7656
- Parent:
- 2:b40c81648155
prototype - 0919
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ohdoyoel | 0:f252870bfd39 | 1 | #include "mbed.h" |
ohdoyoel | 1:fa0730bf53ef | 2 | #include "QEI.h" |
ohdoyoel | 1:fa0730bf53ef | 3 | #include "PID.h" |
ohdoyoel | 0:f252870bfd39 | 4 | |
ohdoyoel | 0:f252870bfd39 | 5 | // serial communication for debugging |
ohdoyoel | 0:f252870bfd39 | 6 | Serial pc(USBTX, USBRX); // tx, rx |
ohdoyoel | 1:fa0730bf53ef | 7 | Ticker debugging_ticker; |
ohdoyoel | 1:fa0730bf53ef | 8 | bool debugging_flag = false; |
ohdoyoel | 1:fa0730bf53ef | 9 | void call_debugging() |
ohdoyoel | 1:fa0730bf53ef | 10 | { |
ohdoyoel | 1:fa0730bf53ef | 11 | debugging_flag = true; |
ohdoyoel | 1:fa0730bf53ef | 12 | } |
ohdoyoel | 1:fa0730bf53ef | 13 | |
ohdoyoel | 0:f252870bfd39 | 14 | |
ohdoyoel | 0:f252870bfd39 | 15 | // global variable |
ohdoyoel | 1:fa0730bf53ef | 16 | #define DT 0.01f |
ohdoyoel | 1:fa0730bf53ef | 17 | #define DT_DEBUGGING 0.05f |
ohdoyoel | 0:f252870bfd39 | 18 | |
ohdoyoel | 0:f252870bfd39 | 19 | // reset btn pin |
ohdoyoel | 0:f252870bfd39 | 20 | DigitalIn reset_btn(D8); |
ohdoyoel | 0:f252870bfd39 | 21 | |
ohdoyoel | 0:f252870bfd39 | 22 | // steering pins |
ohdoyoel | 0:f252870bfd39 | 23 | AnalogIn steering_potent(A0); |
ohdoyoel | 0:f252870bfd39 | 24 | DigitalOut steering_go_sign(D3); |
ohdoyoel | 0:f252870bfd39 | 25 | DigitalOut steering_dir_sign(D4); |
ohdoyoel | 0:f252870bfd39 | 26 | PwmOut steering_pwm(D7); |
ohdoyoel | 0:f252870bfd39 | 27 | Ticker steering_ticker; |
ohdoyoel | 1:fa0730bf53ef | 28 | #define STEERING_PULSE_PER_ROTATION 304000 |
ohdoyoel | 1:fa0730bf53ef | 29 | QEI steering_qei(D5, D6, NC, STEERING_PULSE_PER_ROTATION); |
ohdoyoel | 1:fa0730bf53ef | 30 | PID steering_pid(20.0f, 15.0f, 0.05f, DT); // kp ki kd 15 10 0.01 |
ohdoyoel | 0:f252870bfd39 | 31 | |
ohdoyoel | 0:f252870bfd39 | 32 | // driving pins |
ohdoyoel | 0:f252870bfd39 | 33 | AnalogOut driving_analog_out(A2); |
ohdoyoel | 0:f252870bfd39 | 34 | Ticker driving_ticker; |
ohdoyoel | 0:f252870bfd39 | 35 | Ticker velocity_ticker; |
ohdoyoel | 1:fa0730bf53ef | 36 | #define DRIVING_PULSE_PER_ROTATION 40 |
ohdoyoel | 2:b40c81648155 | 37 | QEI driving_qei(D10, D9, NC, DRIVING_PULSE_PER_ROTATION); |
ohdoyoel | 2:b40c81648155 | 38 | PID driving_pid(0.03f, 0.0f, 0.0005f, DT); |
ohdoyoel | 3:3f559b3f7656 | 39 | DigitalOut driving_break(D11); |
ohdoyoel | 0:f252870bfd39 | 40 | |
ohdoyoel | 0:f252870bfd39 | 41 | // stop btn pin |
ohdoyoel | 0:f252870bfd39 | 42 | DigitalIn stop_btn(D2); |
ohdoyoel | 0:f252870bfd39 | 43 | |
ohdoyoel | 2:b40c81648155 | 44 | bool calibrationing = true; |
ohdoyoel | 2:b40c81648155 | 45 | |
ohdoyoel | 0:f252870bfd39 | 46 | // reset btn function |
ohdoyoel | 0:f252870bfd39 | 47 | void calibration() |
ohdoyoel | 0:f252870bfd39 | 48 | { |
ohdoyoel | 0:f252870bfd39 | 49 | |
ohdoyoel | 2:b40c81648155 | 50 | steering_dir_sign = 0; |
ohdoyoel | 0:f252870bfd39 | 51 | steering_go_sign = 1; |
ohdoyoel | 2:b40c81648155 | 52 | steering_pwm.write(0.5f); |
ohdoyoel | 0:f252870bfd39 | 53 | while (calibrationing) |
ohdoyoel | 0:f252870bfd39 | 54 | { |
ohdoyoel | 0:f252870bfd39 | 55 | if (!reset_btn) calibrationing = false; |
ohdoyoel | 0:f252870bfd39 | 56 | wait(0.01f); |
ohdoyoel | 0:f252870bfd39 | 57 | } |
ohdoyoel | 2:b40c81648155 | 58 | steering_qei.reset(); // reset after calibrationing |
ohdoyoel | 2:b40c81648155 | 59 | steering_dir_sign = 1; |
ohdoyoel | 3:3f559b3f7656 | 60 | while(steering_qei.getPulses() > STEERING_PULSE_PER_ROTATION * (-50.0f / 360.0f)) {} |
ohdoyoel | 2:b40c81648155 | 61 | steering_qei.reset(); // reset after calibrationing |
ohdoyoel | 0:f252870bfd39 | 62 | return ; |
ohdoyoel | 0:f252870bfd39 | 63 | } |
ohdoyoel | 0:f252870bfd39 | 64 | |
ohdoyoel | 0:f252870bfd39 | 65 | // steering functions |
ohdoyoel | 0:f252870bfd39 | 66 | int encoder_count = 0; |
ohdoyoel | 0:f252870bfd39 | 67 | float accumulated_error = 0.0f; |
ohdoyoel | 0:f252870bfd39 | 68 | float delta_error = 0.0f; |
ohdoyoel | 0:f252870bfd39 | 69 | float error_prev = 0.0f; |
ohdoyoel | 0:f252870bfd39 | 70 | float error_ = 0.0f; |
ohdoyoel | 1:fa0730bf53ef | 71 | float threshold = 0.000001f; |
ohdoyoel | 0:f252870bfd39 | 72 | bool steering_flag = false; |
ohdoyoel | 1:fa0730bf53ef | 73 | |
ohdoyoel | 1:fa0730bf53ef | 74 | void call_steering() |
ohdoyoel | 1:fa0730bf53ef | 75 | { |
ohdoyoel | 1:fa0730bf53ef | 76 | steering_flag = true; |
ohdoyoel | 1:fa0730bf53ef | 77 | } |
ohdoyoel | 1:fa0730bf53ef | 78 | |
ohdoyoel | 0:f252870bfd39 | 79 | |
ohdoyoel | 0:f252870bfd39 | 80 | void encoder_counter () |
ohdoyoel | 0:f252870bfd39 | 81 | { |
ohdoyoel | 1:fa0730bf53ef | 82 | encoder_count = steering_qei.getPulses(); |
ohdoyoel | 0:f252870bfd39 | 83 | } |
ohdoyoel | 0:f252870bfd39 | 84 | |
ohdoyoel | 3:3f559b3f7656 | 85 | float potent_to_degree(int deg_min, int deg_max, float potent_min, float potent_max, float potent_val) |
ohdoyoel | 0:f252870bfd39 | 86 | { |
ohdoyoel | 3:3f559b3f7656 | 87 | if (potent_val > potent_max) potent_val = potent_max; |
ohdoyoel | 3:3f559b3f7656 | 88 | else if (potent_val < potent_min) potent_val = potent_min; |
ohdoyoel | 3:3f559b3f7656 | 89 | return ((deg_max - deg_min) / (potent_max - potent_min)) * (potent_val - potent_min) + deg_min; |
ohdoyoel | 0:f252870bfd39 | 90 | } |
ohdoyoel | 0:f252870bfd39 | 91 | |
ohdoyoel | 0:f252870bfd39 | 92 | // 76000 pulse = 360 degrees |
ohdoyoel | 0:f252870bfd39 | 93 | float count_to_degree(int cnt) |
ohdoyoel | 0:f252870bfd39 | 94 | { |
ohdoyoel | 1:fa0730bf53ef | 95 | return (360.0f / STEERING_PULSE_PER_ROTATION) * cnt; |
ohdoyoel | 0:f252870bfd39 | 96 | } |
ohdoyoel | 0:f252870bfd39 | 97 | |
ohdoyoel | 0:f252870bfd39 | 98 | float clip(float x) |
ohdoyoel | 0:f252870bfd39 | 99 | { |
ohdoyoel | 0:f252870bfd39 | 100 | if (-1.0f < x && x < 1.0f) return x; |
ohdoyoel | 0:f252870bfd39 | 101 | else if (x >= 1.0f) return 1.0f; |
ohdoyoel | 0:f252870bfd39 | 102 | else return -1.0f; |
ohdoyoel | 0:f252870bfd39 | 103 | } |
ohdoyoel | 0:f252870bfd39 | 104 | |
ohdoyoel | 0:f252870bfd39 | 105 | void steering() |
ohdoyoel | 0:f252870bfd39 | 106 | { |
ohdoyoel | 3:3f559b3f7656 | 107 | float target_deg = 0.0f; |
ohdoyoel | 3:3f559b3f7656 | 108 | float current_deg = 0.0f; |
ohdoyoel | 1:fa0730bf53ef | 109 | float output = 0.0f; |
ohdoyoel | 1:fa0730bf53ef | 110 | float pid_output = 0.0f; |
ohdoyoel | 1:fa0730bf53ef | 111 | float potent = 0.0f; |
ohdoyoel | 1:fa0730bf53ef | 112 | |
ohdoyoel | 0:f252870bfd39 | 113 | potent = steering_potent.read(); |
ohdoyoel | 3:3f559b3f7656 | 114 | target_deg = potent_to_degree(-40, 40, 0.1f, 0.9f, potent); // direction change는 min max change |
ohdoyoel | 1:fa0730bf53ef | 115 | |
ohdoyoel | 1:fa0730bf53ef | 116 | encoder_counter(); |
ohdoyoel | 0:f252870bfd39 | 117 | current_deg = count_to_degree(encoder_count); |
ohdoyoel | 1:fa0730bf53ef | 118 | |
ohdoyoel | 1:fa0730bf53ef | 119 | steering_pid.ReadCurrentValue(current_deg); |
ohdoyoel | 1:fa0730bf53ef | 120 | steering_pid.SetTargetValue(target_deg); |
ohdoyoel | 1:fa0730bf53ef | 121 | |
ohdoyoel | 1:fa0730bf53ef | 122 | pid_output = steering_pid.Performance_Location(); |
ohdoyoel | 0:f252870bfd39 | 123 | output = clip(pid_output); |
ohdoyoel | 0:f252870bfd39 | 124 | |
ohdoyoel | 0:f252870bfd39 | 125 | if (output > threshold) |
ohdoyoel | 0:f252870bfd39 | 126 | { |
ohdoyoel | 0:f252870bfd39 | 127 | steering_go_sign = 1; |
ohdoyoel | 1:fa0730bf53ef | 128 | steering_dir_sign = 0; |
ohdoyoel | 0:f252870bfd39 | 129 | steering_pwm.write(output); |
ohdoyoel | 3:3f559b3f7656 | 130 | } |
ohdoyoel | 0:f252870bfd39 | 131 | else if (output < (-1 * threshold)) |
ohdoyoel | 0:f252870bfd39 | 132 | { |
ohdoyoel | 0:f252870bfd39 | 133 | steering_go_sign = 1; |
ohdoyoel | 1:fa0730bf53ef | 134 | steering_dir_sign = 1; |
ohdoyoel | 0:f252870bfd39 | 135 | steering_pwm.write(-1 * output); |
ohdoyoel | 0:f252870bfd39 | 136 | } |
ohdoyoel | 0:f252870bfd39 | 137 | else |
ohdoyoel | 0:f252870bfd39 | 138 | { |
ohdoyoel | 0:f252870bfd39 | 139 | steering_dir_sign = 0; |
ohdoyoel | 0:f252870bfd39 | 140 | steering_go_sign = 0; |
ohdoyoel | 0:f252870bfd39 | 141 | } |
ohdoyoel | 0:f252870bfd39 | 142 | } |
ohdoyoel | 0:f252870bfd39 | 143 | |
ohdoyoel | 0:f252870bfd39 | 144 | // driving functions |
ohdoyoel | 0:f252870bfd39 | 145 | bool driving_flag = false; |
ohdoyoel | 0:f252870bfd39 | 146 | int hall_count = 0; |
ohdoyoel | 0:f252870bfd39 | 147 | float current_velocity = 0; |
ohdoyoel | 1:fa0730bf53ef | 148 | float max_velocity = 0; |
ohdoyoel | 0:f252870bfd39 | 149 | float target_velocity = 0; |
ohdoyoel | 0:f252870bfd39 | 150 | float writing_velocity = 0; // 0~1 |
ohdoyoel | 2:b40c81648155 | 151 | float output = 0.0f; |
ohdoyoel | 0:f252870bfd39 | 152 | float velocity_threshold = 0.05f; |
ohdoyoel | 0:f252870bfd39 | 153 | float step = 0.01f; |
ohdoyoel | 0:f252870bfd39 | 154 | int prev_step = 0; |
ohdoyoel | 0:f252870bfd39 | 155 | |
ohdoyoel | 0:f252870bfd39 | 156 | void call_driving() |
ohdoyoel | 0:f252870bfd39 | 157 | { |
ohdoyoel | 0:f252870bfd39 | 158 | driving_flag = true; |
ohdoyoel | 0:f252870bfd39 | 159 | } |
ohdoyoel | 0:f252870bfd39 | 160 | |
ohdoyoel | 0:f252870bfd39 | 161 | void hall_counter() |
ohdoyoel | 0:f252870bfd39 | 162 | { |
ohdoyoel | 1:fa0730bf53ef | 163 | hall_count = driving_qei.getPulses(); |
ohdoyoel | 0:f252870bfd39 | 164 | } |
ohdoyoel | 0:f252870bfd39 | 165 | |
ohdoyoel | 0:f252870bfd39 | 166 | void calc_velocity() |
ohdoyoel | 0:f252870bfd39 | 167 | { |
ohdoyoel | 1:fa0730bf53ef | 168 | current_velocity = hall_count / DT; |
ohdoyoel | 1:fa0730bf53ef | 169 | driving_qei.reset(); |
ohdoyoel | 0:f252870bfd39 | 170 | } |
ohdoyoel | 0:f252870bfd39 | 171 | |
ohdoyoel | 0:f252870bfd39 | 172 | void driving() |
ohdoyoel | 0:f252870bfd39 | 173 | { |
ohdoyoel | 1:fa0730bf53ef | 174 | float pid_output = 0.0f; |
ohdoyoel | 1:fa0730bf53ef | 175 | |
ohdoyoel | 1:fa0730bf53ef | 176 | hall_counter(); |
ohdoyoel | 1:fa0730bf53ef | 177 | calc_velocity(); |
ohdoyoel | 1:fa0730bf53ef | 178 | |
ohdoyoel | 2:b40c81648155 | 179 | max_velocity = max_velocity < current_velocity ? current_velocity : max_velocity; |
ohdoyoel | 1:fa0730bf53ef | 180 | |
ohdoyoel | 3:3f559b3f7656 | 181 | if (max_velocity < 200) target_velocity = 0; |
ohdoyoel | 3:3f559b3f7656 | 182 | else if (max_velocity < 600) target_velocity = 400; |
ohdoyoel | 3:3f559b3f7656 | 183 | else if (max_velocity < 1000) target_velocity = 800; |
ohdoyoel | 3:3f559b3f7656 | 184 | else target_velocity = 1200; |
ohdoyoel | 1:fa0730bf53ef | 185 | |
ohdoyoel | 1:fa0730bf53ef | 186 | driving_pid.ReadCurrentValue(current_velocity); |
ohdoyoel | 1:fa0730bf53ef | 187 | driving_pid.SetTargetValue(target_velocity); |
ohdoyoel | 2:b40c81648155 | 188 | pid_output = driving_pid.Performance_Location(); |
ohdoyoel | 2:b40c81648155 | 189 | output += pid_output; |
ohdoyoel | 3:3f559b3f7656 | 190 | |
ohdoyoel | 3:3f559b3f7656 | 191 | // clip |
ohdoyoel | 2:b40c81648155 | 192 | if (output > 1.0f) output = 1.0f; |
ohdoyoel | 2:b40c81648155 | 193 | else if (output < 0.0f) output = 0.0f; |
ohdoyoel | 1:fa0730bf53ef | 194 | |
ohdoyoel | 2:b40c81648155 | 195 | // dead zone: 0.38 pwm |
ohdoyoel | 2:b40c81648155 | 196 | writing_velocity = 0.7f * output + 0.3f; |
ohdoyoel | 1:fa0730bf53ef | 197 | |
ohdoyoel | 2:b40c81648155 | 198 | driving_analog_out.write(writing_velocity); //writing_velocity |
ohdoyoel | 2:b40c81648155 | 199 | |
ohdoyoel | 2:b40c81648155 | 200 | pc.printf("%d,%f,%f,%f,%f\r", hall_count, current_velocity, target_velocity, pid_output, writing_velocity); |
ohdoyoel | 0:f252870bfd39 | 201 | } |
ohdoyoel | 0:f252870bfd39 | 202 | |
ohdoyoel | 0:f252870bfd39 | 203 | // stop btn function |
ohdoyoel | 0:f252870bfd39 | 204 | void stop_btn_pressed() |
ohdoyoel | 0:f252870bfd39 | 205 | { |
ohdoyoel | 3:3f559b3f7656 | 206 | driving_break = 1; |
ohdoyoel | 3:3f559b3f7656 | 207 | target_velocity = 0.0f; |
ohdoyoel | 0:f252870bfd39 | 208 | writing_velocity = 0.0f; |
ohdoyoel | 3:3f559b3f7656 | 209 | output = 0.0f; |
ohdoyoel | 3:3f559b3f7656 | 210 | max_velocity = 0.0f; |
ohdoyoel | 0:f252870bfd39 | 211 | } |
ohdoyoel | 0:f252870bfd39 | 212 | |
ohdoyoel | 0:f252870bfd39 | 213 | int main () |
ohdoyoel | 0:f252870bfd39 | 214 | { |
ohdoyoel | 1:fa0730bf53ef | 215 | // debugging |
ohdoyoel | 1:fa0730bf53ef | 216 | debugging_ticker.attach(&call_debugging, DT_DEBUGGING); |
ohdoyoel | 1:fa0730bf53ef | 217 | |
ohdoyoel | 0:f252870bfd39 | 218 | //steering setting |
ohdoyoel | 0:f252870bfd39 | 219 | steering_go_sign = 0; |
ohdoyoel | 1:fa0730bf53ef | 220 | steering_dir_sign = 1; |
ohdoyoel | 1:fa0730bf53ef | 221 | steering_pwm.period(0.00001f); |
ohdoyoel | 1:fa0730bf53ef | 222 | steering_ticker.attach(&call_steering, DT); |
ohdoyoel | 0:f252870bfd39 | 223 | |
ohdoyoel | 0:f252870bfd39 | 224 | //driving setting |
ohdoyoel | 1:fa0730bf53ef | 225 | driving_ticker.attach(&call_driving, DT); |
ohdoyoel | 1:fa0730bf53ef | 226 | driving_qei.reset(); |
ohdoyoel | 0:f252870bfd39 | 227 | |
ohdoyoel | 0:f252870bfd39 | 228 | // calibration |
ohdoyoel | 2:b40c81648155 | 229 | calibration(); |
ohdoyoel | 2:b40c81648155 | 230 | |
ohdoyoel | 0:f252870bfd39 | 231 | // driving |
ohdoyoel | 0:f252870bfd39 | 232 | while(1) |
ohdoyoel | 0:f252870bfd39 | 233 | { |
ohdoyoel | 1:fa0730bf53ef | 234 | if (debugging_flag) |
ohdoyoel | 1:fa0730bf53ef | 235 | { |
ohdoyoel | 1:fa0730bf53ef | 236 | debugging_flag = false; |
ohdoyoel | 3:3f559b3f7656 | 237 | pc.printf("%f\r", steering_potent.read()); |
ohdoyoel | 1:fa0730bf53ef | 238 | // pc.printf("%d\r", hall_count); |
ohdoyoel | 2:b40c81648155 | 239 | //pc.printf("%f,%f\r", target_deg, current_deg); |
ohdoyoel | 1:fa0730bf53ef | 240 | //pc.printf("%d\r", encoder_count); |
ohdoyoel | 3:3f559b3f7656 | 241 | //pc.printf("%f,%f,%f\n", current_velocity, target_velocity, writing_velocity);\ |
ohdoyoel | 1:fa0730bf53ef | 242 | } |
ohdoyoel | 1:fa0730bf53ef | 243 | |
ohdoyoel | 1:fa0730bf53ef | 244 | // steering_go_sign = 1; |
ohdoyoel | 1:fa0730bf53ef | 245 | // steering_dir_sign = 0; |
ohdoyoel | 1:fa0730bf53ef | 246 | // steering_pwm.write(steering_potent.read()); |
ohdoyoel | 0:f252870bfd39 | 247 | //if(!reset_btn) pc.printf("reset!"); |
ohdoyoel | 3:3f559b3f7656 | 248 | if(!stop_btn) stop_btn_pressed(); |
ohdoyoel | 3:3f559b3f7656 | 249 | else driving_break = 0; |
ohdoyoel | 0:f252870bfd39 | 250 | if(steering_flag) |
ohdoyoel | 0:f252870bfd39 | 251 | { |
ohdoyoel | 0:f252870bfd39 | 252 | steering_flag = false; |
ohdoyoel | 0:f252870bfd39 | 253 | steering(); |
ohdoyoel | 0:f252870bfd39 | 254 | } |
ohdoyoel | 3:3f559b3f7656 | 255 | if(driving_flag) |
ohdoyoel | 3:3f559b3f7656 | 256 | { |
ohdoyoel | 3:3f559b3f7656 | 257 | driving_flag = false; |
ohdoyoel | 3:3f559b3f7656 | 258 | driving(); |
ohdoyoel | 3:3f559b3f7656 | 259 | } |
ohdoyoel | 0:f252870bfd39 | 260 | } |
ohdoyoel | 3:3f559b3f7656 | 261 | } |
ohdoyoel | 0:f252870bfd39 | 262 | } |