Diff: main.cpp

Revision:

17:baffdedf9590

Parent:

16:53d3445dcf6b

Child:

18:01977903e972

diff -r 53d3445dcf6b -r baffdedf9590 main.cpp
--- 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++;