Diff: main.cpp

Revision:

19:e95f6004f365

Parent:

18:01977903e972

Child:

20:4371ed979fbf

--- a/main.cpp	Sun Mar 08 11:30:57 2020 +0000
+++ b/main.cpp	Fri Mar 13 14:29:58 2020 +0000
@@ -71,7 +71,7 @@
 const float kd_pos = 0;
 const float pwm_period  =0.25f;
 
-int target_velocity = 30;
+int target_velocity = -30;
 float target_position = 500;
 
 // SHARED GLOBAL VARIABLES //
@@ -262,17 +262,27 @@
     motorOut(0);
     wait(3.0);
     orState = readRotorState();
-    
+    attach_ISR();
+
     if(target_velocity > 0){
         lead = 2;
         motorOut(1);
+        wait(0.1); 
+        motorOut(2); 
+        wait(0.1); 
+        motorOut(3); 
     }
     else{
         lead = -2;
-        motorOut(5);
+        motorOut(5); 
+        wait(0.3); 
+        motorOut(4);
+        wait(0.3); 
+        motorOut(3); 
+        wait(0.3); 
+        motorOut(4); 
+
     }
-    attach_ISR();
-    
     current_position = get_current_position();
     previous_position = current_position;
     timer_velocity.start();
@@ -282,7 +292,6 @@
 {    
     while(1) {
         thread_motorCtrl.signal_wait(0x1);
-        
         current_position = get_current_position();
         float velocity_factor = (1000/(timer_velocity.read_ms()-last_time_MtrCtlr));
         velocity = ((current_position - previous_position)/6)*velocity_factor;