Luccas Eagles
/
CONTROL_MOTOR_MELODY
added melody to old motorcontol
Diff: main.cpp
- Revision:
- 17:baffdedf9590
- Parent:
- 16:53d3445dcf6b
- Child:
- 18:01977903e972
--- a/main.cpp Sun Mar 08 11:01:22 2020 +0000 +++ b/main.cpp Sun Mar 08 11:13:54 2020 +0000 @@ -32,8 +32,8 @@ // motor controller variables -float position_local = 0; -int position_old = 0; +float current_position = 0; +int previous_position = 0; float velocity = 0; Timer timer_velocity; uint32_t last_time_MtrCtlr; @@ -65,8 +65,8 @@ const float kd_pos = 0; const float pwm_period =0.25f; -int v_ideal = 30; -float pos_goal = 500; +int target_velocity = 30; +float target_position = 500; // SHARED GLOBAL VARIABLES // Semaphore pos_semaphore(0); @@ -170,9 +170,9 @@ //osSignalSet(thread_motorCtrl, 0x1); } -float get_y_rot(float rot_error) +float get_velocity_out(float velocity_error) { - return kp_rot*(rot_error); + return kp_rot*(velocity_error); } float get_y_pos(float e, float change_e) @@ -180,21 +180,21 @@ return kp_pos*e + kd_pos*change_e; } -float get_motor_out(float y) +float get_motor_out(float velocity_out) { float motor_out; - if( y < 0) { - motor_out = y*-1; + if( velocity_out < 0) { + motor_out = velocity_out*-1; } else { - motor_out = y; + motor_out = velocity_out; } - if (y > 1 || y < -1) { + if (velocity_out > 1 || velocity_out < -1) { motor_out = 1; } return motor_out; } -int get_position_local(){ +int get_current_position(){ pos_semaphore.wait(); return position; } @@ -209,30 +209,30 @@ } -float combine_speed(float y_pos, float y_rot, float error_rot) +float combine_speed(float y_pos, float velocity_out, float velocity_error) { - if(error_rot >= 0) { - if(y_pos < y_rot) { + if(velocity_error >= 0) { + if(y_pos < velocity_out) { return y_pos; } else { - return y_rot; + return velocity_out; } } else { - if(y_pos > y_rot){ + if(y_pos > velocity_out){ return y_pos; } else{ - return y_rot; + return velocity_out; } } return y_pos; } -void update_lead(float y){ +void update_lead(float velocity_out){ // No functionality for breaking - if(y >= 0){ + if(velocity_out >= 0){ lead = 2; } else { @@ -243,7 +243,7 @@ void motorInitSequence() { motorCtrlTicker.attach_us(&motorCtrlTick,100000); - pos_error_old = pos_goal; + pos_error_old = target_position; last_time_MtrCtlr = 0; MotorPWM.write(1); @@ -253,7 +253,7 @@ wait(3.0); orState = readRotorState(); - if(v_ideal > 0){ + if(target_velocity > 0){ lead = 2; motorOut(1); } @@ -263,8 +263,8 @@ } attach_ISR(); - position_local = get_position_local(); - position_old = position_local; + current_position = get_current_position(); + previous_position = current_position; timer_velocity.start(); } @@ -273,23 +273,23 @@ while(1) { thread_motorCtrl.signal_wait(0x1); - position_local = get_position_local(); + current_position = get_current_position(); float velocity_factor = (1000/(timer_velocity.read_ms()-last_time_MtrCtlr)); - velocity = ((position_local - position_old)/6)*velocity_factor; + velocity = ((current_position - previous_position)/6)*velocity_factor; last_time_MtrCtlr = timer_velocity.read_ms(); - position_old = position_local; + previous_position = current_position; - float error_rot = v_ideal - velocity; - float y_rot = get_y_rot(error_rot); - float motor_out = get_motor_out(y_rot); + float velocity_error = target_velocity - velocity; + float velocity_out = get_velocity_out(velocity_error); + float motor_out = get_motor_out(velocity_out); - update_lead(y_rot); + update_lead(velocity_out); MotorPWM.period(pwm_period); MotorPWM.write(motor_out); if(i > 10) { - pc.printf("Velocity = %f, Position = %f, MotorOut = %f, y = %f, lead = %d\r\n", velocity, position_local, motor_out, y_rot, lead); + pc.printf("Velocity = %f, Position = %f, MotorOut = %f, y = %f, lead = %d\r\n", velocity, current_position, motor_out, velocity_out, lead); i = 0; } i++;