Diff: main.cpp

Revision:

5:892531e4e015

Parent:

4:34ad002cb646

Child:

6:b526cf83fd4f

--- a/main.cpp	Mon Oct 29 09:55:04 2018 +0000
+++ b/main.cpp	Tue Oct 30 08:24:14 2018 +0000
@@ -25,15 +25,15 @@
 
 
 
-DigitalOut motor1_direction(D4);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
+DigitalOut motor1_direction(D4);// draairichting motor 1 (0 is CCW )
 PwmOut motor1_speed_control(D5);//aanstuursnelheid motor 1
 PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2
-DigitalOut motor2_direction(D7);// draairichting motor 2 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
+DigitalOut motor2_direction(D7);// draairichting motor 2 (0 is CCW )
 
-float kp1 = 2;
-    float kp2 = 2;
-    float ki1 = 0.5;
-    float ki2 = 0.5;
+    float kp1 = 0.2;
+    float kp2 = 0.2;
+    float ki1 = 0;
+    float ki2 = 0;
     float u1 = 0;
     float u2 = 0;
     
@@ -61,7 +61,7 @@
 Ticker      loop_timer;
 Ticker      sample_timer;
 Ticker      sample_timer2;
-//HIDScope    scope(5);
+//HIDScope    scope(2);
 
 AnalogIn raw_emg1_input(A0);//input for first emg signal 1, for the modes
 AnalogIn raw_emg2_input(A1);//input for first emg signal 2, for the strength
@@ -114,15 +114,14 @@
     deg_m1 = deg_m1 + counts_m1*(360/(full_ratio));
     deg_m2 = deg_m2 + counts_m2*(360/(full_ratio));
     counts_m1_prev = Encoder1.getPulses();
-    counts_m2_prev = Encoder2.getPulses();    
-            
+    counts_m2_prev = Encoder2.getPulses();                
             
 }
 
 void scopedata()
 {
-    //scope.set(0,emg1_input); // 
-    //scope.set(1,emg1_input); //    
+    //scope.set(0,deg_m1); // 
+    //scope.set(1,deg_m2); //    
     //scope.set(2,emg1_input); //
     //scope.set(3,emg1_input);//
     //scope.set(4,filteredsignal1);
@@ -209,10 +208,10 @@
     }
     
     
- 
     
     int deriv1 = deg_m1 - count1;
     int deriv2 = deg_m2 - count2;
+    
     count1 = deg_m1;
     count2 = deg_m2;
    
@@ -222,12 +221,14 @@
         current_state = homing;
         timer.reset();
         state_changed = true;
-        
-        deg_m1 = -45;
-        deg_m2 = -70;
+        wait(3);
+        deg_m1 = -2;
+        deg_m2 = -2;
     }
     }
     
+float wu1;
+float wu2;  
 void do_state_homing(){
      if (state_changed==true) {
         state_changed = false;        
@@ -236,21 +237,34 @@
     float werror1 = deg_m1 - 0;
     float werror2 = deg_m2 - 0;
     
-   
-   float  wu1 = kp1*werror1 + (u1 + werror1*0.002)*ki1;
-    float wu2 = kp2*werror1 + (u2 + werror1*0.002)*ki2;
+    if(werror1 > 5){
+        wu1 = 1;        }
+    if(werror1 < -5){
+        wu1 = -1;        }
+    else{
+        wu1 = kp1*werror1 + (u1 + werror1*0.002)*ki1;   
+    }
     
-    motor1_speed_control = fabs(wu1/200);
+    if(werror2 > 5){
+        wu2 = 1;}
+    if(werror2 < -5){
+        wu2 = -1;}
+    else{
+        wu2 = (kp2*werror2 + (u2 + werror2*0.002)*ki2);
+        }
+    
+    motor1_speed_control = fabs(wu1)/5 + 0.2;
+    
     if(wu1 > 0){
         motor1_direction = 0;}
     if(wu1< 0){
         motor1_direction = 1;}
 
-    motor2_speed_control = fabs(wu2/200);
+    motor2_speed_control = fabs(wu2)/5 + 0.2;
     
     if(wu2 > 0){
         motor2_direction = 0;}
-    if(wu2< 0){
+    if(wu2 < 0){
         motor2_direction = 1;}
         
 
@@ -323,8 +337,7 @@
     
 void motor_controller(){
     
-    
-   
+  
     
     float jacobian[4];
     float inv_jacobian[4];
@@ -396,9 +409,9 @@
 {        
    // motor1_speed_control.period_us(60); //60 microseconds PWM period, 16.7 kHz
     
-    motor1_direction = 1; //set motor 1 to run counterclockwise (positive) direction for calibration
+    motor1_direction = 0; //set motor 1 to run clockwisedirection for calibration
     motor1_speed_control = 0.25;//to make sure the motor will not run at too high velocity
-    motor2_direction = 0; // set motor 2 to run clockwise (negative) direction
+    motor2_direction = 0; // set motor 2 to run clockwise direction
     motor2_speed_control = 0.25; 
      
     led_red = 0;