DECS_YoungCHA / Mbed 2 deprecated BTS_dc_motor

Dependencies:   mbed

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?

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 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 }