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@0:f252870bfd39, 2022-08-04 (annotated)

Committer:
ohdoyoel
Date:
Thu Aug 04 08:01:11 2022 +0000
Revision:
0:f252870bfd39
Child:
1:fa0730bf53ef
init

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ohdoyoel 0:f252870bfd39 1 #include "mbed.h"
ohdoyoel 0:f252870bfd39 2
ohdoyoel 0:f252870bfd39 3 // serial communication for debugging
ohdoyoel 0:f252870bfd39 4 Serial pc(USBTX, USBRX); // tx, rx
ohdoyoel 0:f252870bfd39 5
ohdoyoel 0:f252870bfd39 6 // global variable
ohdoyoel 0:f252870bfd39 7 float dt = 0.01f; //0.002f;
ohdoyoel 0:f252870bfd39 8
ohdoyoel 0:f252870bfd39 9 // reset btn pin
ohdoyoel 0:f252870bfd39 10 DigitalIn reset_btn(D8);
ohdoyoel 0:f252870bfd39 11
ohdoyoel 0:f252870bfd39 12 // steering pins
ohdoyoel 0:f252870bfd39 13 AnalogIn steering_potent(A0);
ohdoyoel 0:f252870bfd39 14 DigitalOut steering_go_sign(D3);
ohdoyoel 0:f252870bfd39 15 DigitalOut steering_dir_sign(D4);
ohdoyoel 0:f252870bfd39 16 PwmOut steering_pwm(D7);
ohdoyoel 0:f252870bfd39 17 InterruptIn steering_encoder_1(D5);
ohdoyoel 0:f252870bfd39 18 InterruptIn steering_encoder_2(D6);
ohdoyoel 0:f252870bfd39 19 Ticker steering_ticker;
ohdoyoel 0:f252870bfd39 20 //Ticker integral_ticker; // i in PID control
ohdoyoel 0:f252870bfd39 21 //Ticker differential_ticker; // d in PID control
ohdoyoel 0:f252870bfd39 22
ohdoyoel 0:f252870bfd39 23 // driving pins
ohdoyoel 0:f252870bfd39 24 AnalogOut driving_analog_out(A2);
ohdoyoel 0:f252870bfd39 25 Ticker driving_ticker;
ohdoyoel 0:f252870bfd39 26 InterruptIn hall_a(D9);
ohdoyoel 0:f252870bfd39 27 InterruptIn hall_b(D10);
ohdoyoel 0:f252870bfd39 28 InterruptIn hall_c(D11);
ohdoyoel 0:f252870bfd39 29 Ticker velocity_ticker;
ohdoyoel 0:f252870bfd39 30
ohdoyoel 0:f252870bfd39 31 // stop btn pin
ohdoyoel 0:f252870bfd39 32 DigitalIn stop_btn(D2);
ohdoyoel 0:f252870bfd39 33
ohdoyoel 0:f252870bfd39 34 // reset btn function
ohdoyoel 0:f252870bfd39 35
ohdoyoel 0:f252870bfd39 36 void calibration()
ohdoyoel 0:f252870bfd39 37 {
ohdoyoel 0:f252870bfd39 38 bool calibrationing = true;
ohdoyoel 0:f252870bfd39 39
ohdoyoel 0:f252870bfd39 40 steering_dir_sign = 1;
ohdoyoel 0:f252870bfd39 41 steering_go_sign = 1;
ohdoyoel 0:f252870bfd39 42 steering_pwm.write(1.0f);
ohdoyoel 0:f252870bfd39 43 while (calibrationing)
ohdoyoel 0:f252870bfd39 44 {
ohdoyoel 0:f252870bfd39 45 if (!reset_btn) calibrationing = false;
ohdoyoel 0:f252870bfd39 46 wait(0.01f);
ohdoyoel 0:f252870bfd39 47 }
ohdoyoel 0:f252870bfd39 48 return ;
ohdoyoel 0:f252870bfd39 49 }
ohdoyoel 0:f252870bfd39 50
ohdoyoel 0:f252870bfd39 51 // steering functions
ohdoyoel 0:f252870bfd39 52 int encoder_count = 0;
ohdoyoel 0:f252870bfd39 53 float accumulated_error = 0.0f;
ohdoyoel 0:f252870bfd39 54 float delta_error = 0.0f;
ohdoyoel 0:f252870bfd39 55 float error_prev = 0.0f;
ohdoyoel 0:f252870bfd39 56 float error_ = 0.0f;
ohdoyoel 0:f252870bfd39 57 float threshold = 0.0f;
ohdoyoel 0:f252870bfd39 58 bool steering_flag = false;
ohdoyoel 0:f252870bfd39 59 float potent = 0.0f;
ohdoyoel 0:f252870bfd39 60 float target_deg = 0.0f;
ohdoyoel 0:f252870bfd39 61 float current_deg = 0.0f;
ohdoyoel 0:f252870bfd39 62 float _error = 0.0f;
ohdoyoel 0:f252870bfd39 63 float pid_output = 0.0f;
ohdoyoel 0:f252870bfd39 64 float output = 0.0f;
ohdoyoel 0:f252870bfd39 65
ohdoyoel 0:f252870bfd39 66 void encoder_counter ()
ohdoyoel 0:f252870bfd39 67 {
ohdoyoel 0:f252870bfd39 68 if (steering_encoder_2 == 1) encoder_count++;
ohdoyoel 0:f252870bfd39 69 else encoder_count--;
ohdoyoel 0:f252870bfd39 70 }
ohdoyoel 0:f252870bfd39 71
ohdoyoel 0:f252870bfd39 72 void integrate_error ()
ohdoyoel 0:f252870bfd39 73 {
ohdoyoel 0:f252870bfd39 74 accumulated_error += dt * error_;
ohdoyoel 0:f252870bfd39 75 }
ohdoyoel 0:f252870bfd39 76
ohdoyoel 0:f252870bfd39 77 void differentiate_error ()
ohdoyoel 0:f252870bfd39 78 {
ohdoyoel 0:f252870bfd39 79 delta_error = (error_ - error_prev) / dt;
ohdoyoel 0:f252870bfd39 80 }
ohdoyoel 0:f252870bfd39 81
ohdoyoel 0:f252870bfd39 82 float potent_to_degree(int deg_min, int deg_max, float potent_val)
ohdoyoel 0:f252870bfd39 83 {
ohdoyoel 0:f252870bfd39 84 return (deg_max - deg_min) * potent_val + deg_min;
ohdoyoel 0:f252870bfd39 85 }
ohdoyoel 0:f252870bfd39 86
ohdoyoel 0:f252870bfd39 87 // 76000 pulse = 360 degrees
ohdoyoel 0:f252870bfd39 88 float count_to_degree(int cnt)
ohdoyoel 0:f252870bfd39 89 {
ohdoyoel 0:f252870bfd39 90 return (360.0f / 76000.0f) * cnt;
ohdoyoel 0:f252870bfd39 91 }
ohdoyoel 0:f252870bfd39 92
ohdoyoel 0:f252870bfd39 93 float calc_pid_output(float k_p, float k_i, float k_d, float e)
ohdoyoel 0:f252870bfd39 94 {
ohdoyoel 0:f252870bfd39 95 return k_p * e + k_i * accumulated_error + k_d * delta_error;
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 call_steering()
ohdoyoel 0:f252870bfd39 106 {
ohdoyoel 0:f252870bfd39 107 steering_flag = true;
ohdoyoel 0:f252870bfd39 108 }
ohdoyoel 0:f252870bfd39 109
ohdoyoel 0:f252870bfd39 110 void steering()
ohdoyoel 0:f252870bfd39 111 {
ohdoyoel 0:f252870bfd39 112 potent = steering_potent.read();
ohdoyoel 0:f252870bfd39 113 target_deg = potent_to_degree(-40, 40, potent); // direction change = min max change
ohdoyoel 0:f252870bfd39 114 current_deg = count_to_degree(encoder_count);
ohdoyoel 0:f252870bfd39 115 error_prev = error_;
ohdoyoel 0:f252870bfd39 116 _error = target_deg - current_deg;
ohdoyoel 0:f252870bfd39 117 error_ = _error;
ohdoyoel 0:f252870bfd39 118 pid_output = calc_pid_output(0.000001f, 0.0f, 0.0f, _error); // 0.05f, 0.01f, 0.0001f, when 0.001f (1kHz)
ohdoyoel 0:f252870bfd39 119 output = clip(pid_output);
ohdoyoel 0:f252870bfd39 120
ohdoyoel 0:f252870bfd39 121 if (output > threshold)
ohdoyoel 0:f252870bfd39 122 {
ohdoyoel 0:f252870bfd39 123 steering_go_sign = 1;
ohdoyoel 0:f252870bfd39 124 steering_dir_sign = 1;
ohdoyoel 0:f252870bfd39 125 steering_pwm.write(output);
ohdoyoel 0:f252870bfd39 126 }
ohdoyoel 0:f252870bfd39 127 else if (output < (-1 * threshold))
ohdoyoel 0:f252870bfd39 128 {
ohdoyoel 0:f252870bfd39 129 steering_go_sign = 1;
ohdoyoel 0:f252870bfd39 130 steering_dir_sign = 0;
ohdoyoel 0:f252870bfd39 131 steering_pwm.write(-1 * output);
ohdoyoel 0:f252870bfd39 132 }
ohdoyoel 0:f252870bfd39 133 else
ohdoyoel 0:f252870bfd39 134 {
ohdoyoel 0:f252870bfd39 135 steering_dir_sign = 0;
ohdoyoel 0:f252870bfd39 136 steering_go_sign = 0;
ohdoyoel 0:f252870bfd39 137 }
ohdoyoel 0:f252870bfd39 138 }
ohdoyoel 0:f252870bfd39 139
ohdoyoel 0:f252870bfd39 140 // driving functions
ohdoyoel 0:f252870bfd39 141 #define HALL_A_STEP 1 //0x001
ohdoyoel 0:f252870bfd39 142 #define HALL_B_STEP 2 //0x010
ohdoyoel 0:f252870bfd39 143 #define HALL_C_STEP 4 //0x100
ohdoyoel 0:f252870bfd39 144
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 0:f252870bfd39 148 float target_velocity = 0;
ohdoyoel 0:f252870bfd39 149 float writing_velocity = 0; // 0~1
ohdoyoel 0:f252870bfd39 150 float velocity_threshold = 0.05f;
ohdoyoel 0:f252870bfd39 151 float step = 0.01f;
ohdoyoel 0:f252870bfd39 152 int prev_hall_a = hall_a.read();
ohdoyoel 0:f252870bfd39 153 int prev_hall_b = hall_b.read();
ohdoyoel 0:f252870bfd39 154 int prev_hall_c = hall_c.read();
ohdoyoel 0:f252870bfd39 155 int prev_step = 0;
ohdoyoel 0:f252870bfd39 156
ohdoyoel 0:f252870bfd39 157 void call_driving()
ohdoyoel 0:f252870bfd39 158 {
ohdoyoel 0:f252870bfd39 159 driving_flag = true;
ohdoyoel 0:f252870bfd39 160 }
ohdoyoel 0:f252870bfd39 161
ohdoyoel 0:f252870bfd39 162 void hall_counter()
ohdoyoel 0:f252870bfd39 163 {
ohdoyoel 0:f252870bfd39 164 if(prev_step != HALL_A_STEP && prev_hall_a == 0 && prev_hall_a != hall_a.read())
ohdoyoel 0:f252870bfd39 165 {
ohdoyoel 0:f252870bfd39 166 hall_count++;
ohdoyoel 0:f252870bfd39 167 prev_step = HALL_A_STEP;
ohdoyoel 0:f252870bfd39 168 }
ohdoyoel 0:f252870bfd39 169 else if(prev_step != HALL_B_STEP && prev_hall_b == 0 && prev_hall_b != hall_b.read())
ohdoyoel 0:f252870bfd39 170 {
ohdoyoel 0:f252870bfd39 171 hall_count++;
ohdoyoel 0:f252870bfd39 172 prev_step = HALL_B_STEP;
ohdoyoel 0:f252870bfd39 173 }
ohdoyoel 0:f252870bfd39 174 else if(prev_step != HALL_C_STEP && prev_hall_c == 0 && prev_hall_c != hall_c.read())
ohdoyoel 0:f252870bfd39 175 {
ohdoyoel 0:f252870bfd39 176 hall_count++;
ohdoyoel 0:f252870bfd39 177 prev_step = HALL_C_STEP;
ohdoyoel 0:f252870bfd39 178 }
ohdoyoel 0:f252870bfd39 179 prev_hall_a = hall_a.read();
ohdoyoel 0:f252870bfd39 180 prev_hall_b = hall_b.read();
ohdoyoel 0:f252870bfd39 181 prev_hall_c = hall_c.read();
ohdoyoel 0:f252870bfd39 182 }
ohdoyoel 0:f252870bfd39 183
ohdoyoel 0:f252870bfd39 184 void calc_velocity()
ohdoyoel 0:f252870bfd39 185 {
ohdoyoel 0:f252870bfd39 186 current_velocity = hall_count / dt;
ohdoyoel 0:f252870bfd39 187 hall_count = 0;
ohdoyoel 0:f252870bfd39 188 }
ohdoyoel 0:f252870bfd39 189
ohdoyoel 0:f252870bfd39 190 void driving()
ohdoyoel 0:f252870bfd39 191 {
ohdoyoel 0:f252870bfd39 192 if (target_velocity < current_velocity) target_velocity = current_velocity;
ohdoyoel 0:f252870bfd39 193 if ((current_velocity < target_velocity - velocity_threshold) && (writing_velocity < 1.0f)) writing_velocity += step;
ohdoyoel 0:f252870bfd39 194 driving_analog_out.write(writing_velocity); //writing_velocity
ohdoyoel 0:f252870bfd39 195 }
ohdoyoel 0:f252870bfd39 196
ohdoyoel 0:f252870bfd39 197 // stop btn function
ohdoyoel 0:f252870bfd39 198 void stop_btn_pressed()
ohdoyoel 0:f252870bfd39 199 {
ohdoyoel 0:f252870bfd39 200 writing_velocity = 0.0f;
ohdoyoel 0:f252870bfd39 201 pc.printf("stop! \r\n");
ohdoyoel 0:f252870bfd39 202 }
ohdoyoel 0:f252870bfd39 203
ohdoyoel 0:f252870bfd39 204 int main ()
ohdoyoel 0:f252870bfd39 205 {
ohdoyoel 0:f252870bfd39 206 //steering setting
ohdoyoel 0:f252870bfd39 207 steering_go_sign = 0;
ohdoyoel 0:f252870bfd39 208 steering_dir_sign = 0;
ohdoyoel 0:f252870bfd39 209 steering_pwm.period(dt);
ohdoyoel 0:f252870bfd39 210 steering_encoder_1.rise(&encoder_counter);
ohdoyoel 0:f252870bfd39 211 steering_ticker.attach(&call_steering, dt);
ohdoyoel 0:f252870bfd39 212
ohdoyoel 0:f252870bfd39 213 //driving setting
ohdoyoel 0:f252870bfd39 214 driving_ticker.attach(&call_driving, dt);
ohdoyoel 0:f252870bfd39 215 velocity_ticker.attach(&calc_velocity, dt);
ohdoyoel 0:f252870bfd39 216 hall_a.mode(PullUp);
ohdoyoel 0:f252870bfd39 217 hall_b.mode(PullUp);
ohdoyoel 0:f252870bfd39 218 hall_c.mode(PullUp);
ohdoyoel 0:f252870bfd39 219 hall_a.rise(&hall_counter);
ohdoyoel 0:f252870bfd39 220 hall_b.rise(&hall_counter);
ohdoyoel 0:f252870bfd39 221 hall_c.rise(&hall_counter);
ohdoyoel 0:f252870bfd39 222
ohdoyoel 0:f252870bfd39 223 // calibration
ohdoyoel 0:f252870bfd39 224 //calibration();
ohdoyoel 0:f252870bfd39 225
ohdoyoel 0:f252870bfd39 226 encoder_count = 0;// 76000 * (45.0f / 360.0f);
ohdoyoel 0:f252870bfd39 227
ohdoyoel 0:f252870bfd39 228 // driving
ohdoyoel 0:f252870bfd39 229 while(1)
ohdoyoel 0:f252870bfd39 230 {
ohdoyoel 0:f252870bfd39 231 //if(!reset_btn) pc.printf("reset!");
ohdoyoel 0:f252870bfd39 232 //if(!stop_btn) stop_btn_pressed();
ohdoyoel 0:f252870bfd39 233 if(steering_flag)
ohdoyoel 0:f252870bfd39 234 {
ohdoyoel 0:f252870bfd39 235 steering_flag = false;
ohdoyoel 0:f252870bfd39 236 integrate_error();
ohdoyoel 0:f252870bfd39 237 differentiate_error();
ohdoyoel 0:f252870bfd39 238 steering();
ohdoyoel 0:f252870bfd39 239 }
ohdoyoel 0:f252870bfd39 240 //if(driving_flag)
ohdoyoel 0:f252870bfd39 241 //{
ohdoyoel 0:f252870bfd39 242 // driving();
ohdoyoel 0:f252870bfd39 243 // driving_flag = false;
ohdoyoel 0:f252870bfd39 244 //}
ohdoyoel 0:f252870bfd39 245 pc.printf("%f,%f\r", target_deg, current_deg);
ohdoyoel 0:f252870bfd39 246 //pc.printf("%f,%f,%f\n", current_velocity, target_velocity, writing_velocity);
ohdoyoel 0:f252870bfd39 247 }
ohdoyoel 0:f252870bfd39 248 }