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@2:b40c81648155, 2022-08-25 (annotated)

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