DECS_YoungCHA / Mbed 2 deprecated BTS_dc_motor

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@1:fa0730bf53ef, 2022-08-23 (annotated)

Committer:
ohdoyoel
Date:
Tue Aug 23 08:55:01 2022 +0000
Revision:
1:fa0730bf53ef
Parent:
0:f252870bfd39
Child:
2:b40c81648155
hello~

Who changed what in which revision?

UserRevisionLine numberNew 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 1:fa0730bf53ef 37 QEI driving_qei(D9, D10, NC, DRIVING_PULSE_PER_ROTATION);
ohdoyoel 1:fa0730bf53ef 38 PID driving_pid(10.0f, 0.0f, 0.0f, DT); // kp ki kd 15 10 0.01
ohdoyoel 0:f252870bfd39 39
ohdoyoel 0:f252870bfd39 40 // stop btn pin
ohdoyoel 0:f252870bfd39 41 DigitalIn stop_btn(D2);
ohdoyoel 0:f252870bfd39 42
ohdoyoel 0:f252870bfd39 43 // reset btn function
ohdoyoel 0:f252870bfd39 44 void calibration()
ohdoyoel 0:f252870bfd39 45 {
ohdoyoel 0:f252870bfd39 46 bool calibrationing = true;
ohdoyoel 0:f252870bfd39 47
ohdoyoel 0:f252870bfd39 48 steering_dir_sign = 1;
ohdoyoel 0:f252870bfd39 49 steering_go_sign = 1;
ohdoyoel 0:f252870bfd39 50 steering_pwm.write(1.0f);
ohdoyoel 0:f252870bfd39 51 while (calibrationing)
ohdoyoel 0:f252870bfd39 52 {
ohdoyoel 0:f252870bfd39 53 if (!reset_btn) calibrationing = false;
ohdoyoel 0:f252870bfd39 54 wait(0.01f);
ohdoyoel 0:f252870bfd39 55 }
ohdoyoel 0:f252870bfd39 56 return ;
ohdoyoel 0:f252870bfd39 57 }
ohdoyoel 0:f252870bfd39 58
ohdoyoel 0:f252870bfd39 59 // steering functions
ohdoyoel 0:f252870bfd39 60 int encoder_count = 0;
ohdoyoel 0:f252870bfd39 61 float accumulated_error = 0.0f;
ohdoyoel 0:f252870bfd39 62 float delta_error = 0.0f;
ohdoyoel 0:f252870bfd39 63 float error_prev = 0.0f;
ohdoyoel 0:f252870bfd39 64 float error_ = 0.0f;
ohdoyoel 1:fa0730bf53ef 65 float threshold = 0.000001f;
ohdoyoel 0:f252870bfd39 66 bool steering_flag = false;
ohdoyoel 1:fa0730bf53ef 67
ohdoyoel 1:fa0730bf53ef 68 void call_steering()
ohdoyoel 1:fa0730bf53ef 69 {
ohdoyoel 1:fa0730bf53ef 70 steering_flag = true;
ohdoyoel 1:fa0730bf53ef 71 }
ohdoyoel 1:fa0730bf53ef 72
ohdoyoel 0:f252870bfd39 73
ohdoyoel 0:f252870bfd39 74 void encoder_counter ()
ohdoyoel 0:f252870bfd39 75 {
ohdoyoel 1:fa0730bf53ef 76 encoder_count = steering_qei.getPulses();
ohdoyoel 0:f252870bfd39 77 }
ohdoyoel 0:f252870bfd39 78
ohdoyoel 0:f252870bfd39 79 float potent_to_degree(int deg_min, int deg_max, float potent_val)
ohdoyoel 0:f252870bfd39 80 {
ohdoyoel 0:f252870bfd39 81 return (deg_max - deg_min) * potent_val + deg_min;
ohdoyoel 0:f252870bfd39 82 }
ohdoyoel 0:f252870bfd39 83
ohdoyoel 0:f252870bfd39 84 // 76000 pulse = 360 degrees
ohdoyoel 0:f252870bfd39 85 float count_to_degree(int cnt)
ohdoyoel 0:f252870bfd39 86 {
ohdoyoel 1:fa0730bf53ef 87 return (360.0f / STEERING_PULSE_PER_ROTATION) * cnt;
ohdoyoel 0:f252870bfd39 88 }
ohdoyoel 0:f252870bfd39 89
ohdoyoel 0:f252870bfd39 90 float clip(float x)
ohdoyoel 0:f252870bfd39 91 {
ohdoyoel 0:f252870bfd39 92 if (-1.0f < x && x < 1.0f) return x;
ohdoyoel 0:f252870bfd39 93 else if (x >= 1.0f) return 1.0f;
ohdoyoel 0:f252870bfd39 94 else return -1.0f;
ohdoyoel 0:f252870bfd39 95 }
ohdoyoel 0:f252870bfd39 96
ohdoyoel 1:fa0730bf53ef 97 float target_deg = 0.0f;
ohdoyoel 1:fa0730bf53ef 98 float current_deg = 0.0f;
ohdoyoel 0:f252870bfd39 99
ohdoyoel 0:f252870bfd39 100 void steering()
ohdoyoel 0:f252870bfd39 101 {
ohdoyoel 1:fa0730bf53ef 102 float output = 0.0f;
ohdoyoel 1:fa0730bf53ef 103 float pid_output = 0.0f;
ohdoyoel 1:fa0730bf53ef 104 float potent = 0.0f;
ohdoyoel 1:fa0730bf53ef 105
ohdoyoel 0:f252870bfd39 106 potent = steering_potent.read();
ohdoyoel 0:f252870bfd39 107 target_deg = potent_to_degree(-40, 40, potent); // direction change = min max change
ohdoyoel 1:fa0730bf53ef 108
ohdoyoel 1:fa0730bf53ef 109 encoder_counter();
ohdoyoel 0:f252870bfd39 110 current_deg = count_to_degree(encoder_count);
ohdoyoel 1:fa0730bf53ef 111
ohdoyoel 1:fa0730bf53ef 112 steering_pid.ReadCurrentValue(current_deg);
ohdoyoel 1:fa0730bf53ef 113 steering_pid.SetTargetValue(target_deg);
ohdoyoel 1:fa0730bf53ef 114
ohdoyoel 1:fa0730bf53ef 115 pid_output = steering_pid.Performance_Location();
ohdoyoel 0:f252870bfd39 116 output = clip(pid_output);
ohdoyoel 0:f252870bfd39 117
ohdoyoel 0:f252870bfd39 118 if (output > threshold)
ohdoyoel 0:f252870bfd39 119 {
ohdoyoel 0:f252870bfd39 120 steering_go_sign = 1;
ohdoyoel 1:fa0730bf53ef 121 steering_dir_sign = 0;
ohdoyoel 0:f252870bfd39 122 steering_pwm.write(output);
ohdoyoel 0:f252870bfd39 123 }
ohdoyoel 0:f252870bfd39 124 else if (output < (-1 * threshold))
ohdoyoel 0:f252870bfd39 125 {
ohdoyoel 0:f252870bfd39 126 steering_go_sign = 1;
ohdoyoel 1:fa0730bf53ef 127 steering_dir_sign = 1;
ohdoyoel 0:f252870bfd39 128 steering_pwm.write(-1 * output);
ohdoyoel 0:f252870bfd39 129 }
ohdoyoel 0:f252870bfd39 130 else
ohdoyoel 0:f252870bfd39 131 {
ohdoyoel 0:f252870bfd39 132 steering_dir_sign = 0;
ohdoyoel 0:f252870bfd39 133 steering_go_sign = 0;
ohdoyoel 0:f252870bfd39 134 }
ohdoyoel 0:f252870bfd39 135 }
ohdoyoel 0:f252870bfd39 136
ohdoyoel 0:f252870bfd39 137 // driving functions
ohdoyoel 0:f252870bfd39 138 bool driving_flag = false;
ohdoyoel 0:f252870bfd39 139 int hall_count = 0;
ohdoyoel 0:f252870bfd39 140 float current_velocity = 0;
ohdoyoel 1:fa0730bf53ef 141 float max_velocity = 0;
ohdoyoel 0:f252870bfd39 142 float target_velocity = 0;
ohdoyoel 0:f252870bfd39 143 float writing_velocity = 0; // 0~1
ohdoyoel 0:f252870bfd39 144 float velocity_threshold = 0.05f;
ohdoyoel 0:f252870bfd39 145 float step = 0.01f;
ohdoyoel 0:f252870bfd39 146 int prev_step = 0;
ohdoyoel 0:f252870bfd39 147
ohdoyoel 0:f252870bfd39 148 void call_driving()
ohdoyoel 0:f252870bfd39 149 {
ohdoyoel 0:f252870bfd39 150 driving_flag = true;
ohdoyoel 0:f252870bfd39 151 }
ohdoyoel 0:f252870bfd39 152
ohdoyoel 0:f252870bfd39 153 void hall_counter()
ohdoyoel 0:f252870bfd39 154 {
ohdoyoel 1:fa0730bf53ef 155 hall_count = driving_qei.getPulses();
ohdoyoel 0:f252870bfd39 156 }
ohdoyoel 0:f252870bfd39 157
ohdoyoel 0:f252870bfd39 158 void calc_velocity()
ohdoyoel 0:f252870bfd39 159 {
ohdoyoel 1:fa0730bf53ef 160 current_velocity = hall_count / DT;
ohdoyoel 1:fa0730bf53ef 161 driving_qei.reset();
ohdoyoel 0:f252870bfd39 162 }
ohdoyoel 0:f252870bfd39 163
ohdoyoel 0:f252870bfd39 164 void driving()
ohdoyoel 0:f252870bfd39 165 {
ohdoyoel 1:fa0730bf53ef 166 float output = 0.0f;
ohdoyoel 1:fa0730bf53ef 167 float pid_output = 0.0f;
ohdoyoel 1:fa0730bf53ef 168
ohdoyoel 1:fa0730bf53ef 169 hall_counter();
ohdoyoel 1:fa0730bf53ef 170 calc_velocity();
ohdoyoel 1:fa0730bf53ef 171
ohdoyoel 1:fa0730bf53ef 172 max_velocity = max_velocity < current_velocity ? currentRPS : max_velocity;
ohdoyoel 1:fa0730bf53ef 173
ohdoyoel 1:fa0730bf53ef 174 if (max_velocity < 200) target_velocity = 100;
ohdoyoel 1:fa0730bf53ef 175 else if (max_velocity < 400) target_velocity = 300;
ohdoyoel 1:fa0730bf53ef 176 else if (max_velocity < 600) target_velocity = 500;
ohdoyoel 1:fa0730bf53ef 177 else if (max_velocity < 800) target_velocity = 700;
ohdoyoel 1:fa0730bf53ef 178 else if (max_velocity < 1000) target_velocity = 900;
ohdoyoel 1:fa0730bf53ef 179 else if (max_velocity < 1200) target_velocity = 1100;
ohdoyoel 1:fa0730bf53ef 180 else target_velocity = 1300;
ohdoyoel 1:fa0730bf53ef 181
ohdoyoel 1:fa0730bf53ef 182 driving_pid.ReadCurrentValue(current_velocity);
ohdoyoel 1:fa0730bf53ef 183 driving_pid.SetTargetValue(target_velocity);
ohdoyoel 1:fa0730bf53ef 184
ohdoyoel 1:fa0730bf53ef 185 pid_output = driving_pid.Performance_Location();
ohdoyoel 1:fa0730bf53ef 186 output = clip(pid_output);
ohdoyoel 1:fa0730bf53ef 187
ohdoyoel 1:fa0730bf53ef 188 pc.printf("%d,%f,%f,%f\r", hall_count, current_velocity, target_velocity, pid_output);
ohdoyoel 1:fa0730bf53ef 189 //driving_analog_out.write(output); //writing_velocity
ohdoyoel 0:f252870bfd39 190 }
ohdoyoel 0:f252870bfd39 191
ohdoyoel 0:f252870bfd39 192 // stop btn function
ohdoyoel 0:f252870bfd39 193 void stop_btn_pressed()
ohdoyoel 0:f252870bfd39 194 {
ohdoyoel 0:f252870bfd39 195 writing_velocity = 0.0f;
ohdoyoel 0:f252870bfd39 196 pc.printf("stop! \r\n");
ohdoyoel 0:f252870bfd39 197 }
ohdoyoel 0:f252870bfd39 198
ohdoyoel 0:f252870bfd39 199 int main ()
ohdoyoel 0:f252870bfd39 200 {
ohdoyoel 1:fa0730bf53ef 201 // debugging
ohdoyoel 1:fa0730bf53ef 202 debugging_ticker.attach(&call_debugging, DT_DEBUGGING);
ohdoyoel 1:fa0730bf53ef 203
ohdoyoel 0:f252870bfd39 204 //steering setting
ohdoyoel 0:f252870bfd39 205 steering_go_sign = 0;
ohdoyoel 1:fa0730bf53ef 206 steering_dir_sign = 1;
ohdoyoel 1:fa0730bf53ef 207 steering_pwm.period(0.00001f);
ohdoyoel 1:fa0730bf53ef 208 steering_ticker.attach(&call_steering, DT);
ohdoyoel 1:fa0730bf53ef 209 steering_qei.reset();
ohdoyoel 0:f252870bfd39 210
ohdoyoel 0:f252870bfd39 211 //driving setting
ohdoyoel 1:fa0730bf53ef 212 driving_ticker.attach(&call_driving, DT);
ohdoyoel 1:fa0730bf53ef 213 //velocity_ticker.attach(&calc_velocity, DT);
ohdoyoel 1:fa0730bf53ef 214 driving_qei.reset();
ohdoyoel 1:fa0730bf53ef 215 // hall_a.mode(PullUp);
ohdoyoel 1:fa0730bf53ef 216 // hall_b.mode(PullUp);
ohdoyoel 1:fa0730bf53ef 217 // hall_a.rise(&hall_counter);
ohdoyoel 1:fa0730bf53ef 218 // hall_b.rise(&hall_counter);
ohdoyoel 0:f252870bfd39 219
ohdoyoel 0:f252870bfd39 220 // calibration
ohdoyoel 0:f252870bfd39 221 //calibration();
ohdoyoel 0:f252870bfd39 222
ohdoyoel 0:f252870bfd39 223 encoder_count = 0;// 76000 * (45.0f / 360.0f);
ohdoyoel 0:f252870bfd39 224
ohdoyoel 0:f252870bfd39 225 // driving
ohdoyoel 0:f252870bfd39 226 while(1)
ohdoyoel 0:f252870bfd39 227 {
ohdoyoel 1:fa0730bf53ef 228 if (debugging_flag)
ohdoyoel 1:fa0730bf53ef 229 {
ohdoyoel 1:fa0730bf53ef 230 debugging_flag = false;
ohdoyoel 1:fa0730bf53ef 231 // pc.printf("%d\r", hall_count);
ohdoyoel 1:fa0730bf53ef 232 //pc.printf("%f,%f,%f\r", target_deg, current_deg, output);
ohdoyoel 1:fa0730bf53ef 233 //pc.printf("%d\r", encoder_count);
ohdoyoel 1:fa0730bf53ef 234 }
ohdoyoel 1:fa0730bf53ef 235
ohdoyoel 1:fa0730bf53ef 236 // steering_go_sign = 1;
ohdoyoel 1:fa0730bf53ef 237 // steering_dir_sign = 0;
ohdoyoel 1:fa0730bf53ef 238 // steering_pwm.write(steering_potent.read());
ohdoyoel 0:f252870bfd39 239 //if(!reset_btn) pc.printf("reset!");
ohdoyoel 0:f252870bfd39 240 //if(!stop_btn) stop_btn_pressed();
ohdoyoel 0:f252870bfd39 241 if(steering_flag)
ohdoyoel 0:f252870bfd39 242 {
ohdoyoel 0:f252870bfd39 243 steering_flag = false;
ohdoyoel 0:f252870bfd39 244 steering();
ohdoyoel 0:f252870bfd39 245 }
ohdoyoel 1:fa0730bf53ef 246 if(driving_flag)
ohdoyoel 1:fa0730bf53ef 247 {
ohdoyoel 1:fa0730bf53ef 248 driving_flag = false;
ohdoyoel 1:fa0730bf53ef 249 driving_analog_out.write(steering_potent.read());
ohdoyoel 1:fa0730bf53ef 250 driving();
ohdoyoel 1:fa0730bf53ef 251 }
ohdoyoel 0:f252870bfd39 252 //pc.printf("%f,%f,%f\n", current_velocity, target_velocity, writing_velocity);
ohdoyoel 0:f252870bfd39 253 }
ohdoyoel 0:f252870bfd39 254 }