Diff: main.cpp

Revision:

26:9e21ce046d4e

Parent:

23:d1a3d537460f

Child:

27:f5b33cbd3c22

--- a/main.cpp	Thu Oct 31 12:11:54 2019 +0000
+++ b/main.cpp	Thu Oct 31 17:24:05 2019 +0000
@@ -50,16 +50,16 @@
     double length_link1=18;
     double length_link2=15;
     double x = 0;   // (of -2?) 8.5 < x < 30.5
-    double x_max= 30.5;
-    double x_min= 8.5;
+    double x_max= 100; //30.5;
+    double x_min= -100; //8.5;
     double x_home=8.5;
-    double x_vergroting=0.3;
+    double x_vergroting=0.002;
     
     double y = 0; // 7.5 < y < 27
-    double y_max=27;
-    double y_min=7.5;
+    double y_max=100; //27;
+    double y_min=-100; //7.5;
     double y_home=7.5;
-    double y_vergroting=0.03;
+    double y_vergroting=0.002;
     double a;
     double b;
     double c;
@@ -99,11 +99,11 @@
     DigitalOut led_red(LED_RED); // Defines the red led on the K64 board (0=on, 1 = off)   
     DigitalOut led_green(LED_GREEN); // Defines the green led on the K64 board (0=on, 1 = off)
     DigitalOut led_blue(LED_BLUE); // Defines the blue led on the K64 board (0=on, 1 = off)  
-//    FastPWM led1(D8);         //CODE DOES NOT WORK WITH D8 PIN DEFINED      //Defines Led1 on the BioRobotics Shield
-    FastPWM led2(D9);               //Defines Led2 on the BioRobotics Shield
+    FastPWM led1(D9);         //CODE DOES NOT WORK WITH D8 PIN DEFINED      //Defines Led1 on the BioRobotics Shield
+    FastPWM led2(A5);               //Defines Led2 on the BioRobotics Shield
     
     //3.4b Potmeters and buttons
-    AnalogIn pot1_links(A5); //Defines potmeter1 on the BioRobotics Shield
+  //  AnalogIn pot1_links(A5); //BUITEN GEBRUIK Defines potmeter1 on the BioRobotics Shield
     AnalogIn pot2_rechts(A4); //Defines potmeter2 on the BioRobotics Shield
     DigitalIn button1(D2); //Defines button1 on the BioRobotics Shield
     DigitalIn button2(D3); //Defines button2 on the BioRobotics Shield
@@ -133,6 +133,7 @@
     int emg_tijd=0; 
     bool calibration_done=false;
     int homing_tijd=0;
+    int demo_tijd=0;
     
 // 3.5 Motor 1 variables ***************************************************************   
     //3.5a PID-controller motor 1
@@ -185,9 +186,12 @@
     // 4.1 Hidscope ****************************************************************
     void HIDScope() //voor HIDscope
     {
-        scope.set(0, emg0_signal);
-        scope.set(1, emg0_max);
-        scope.set(2, x);
+        scope.set(0, emg2_signal);
+        scope.set(1, emg3_signal);
+   //     scope.set(2, emg2_raw_signal);
+   //     scope.set(3, emg3_raw_signal);
+ 
+
    //    scope.set(4, Ui_motor1);
    //    scope.set(5, Uk_motor1);
        
@@ -217,10 +221,10 @@
         
     void motor_calibration()
     {
-        // Calibration motor 2
+        // Calibration motor 1
         motor1DirectionPin=0; //direction of the motor
         motor1=1.0;
-        wait(0.1);
+        wait(0.3);
         while (abs(positie_verschil_motor1)>5)
         {
             motor1=0.2  ; 
@@ -235,7 +239,7 @@
         // Calibration motor 2
         motor2DirectionPin=0; //direction of the motor
         motor2=1.0;
-        wait(0.1);
+        wait(0.3);
         while (abs(positie_verschil_motor2)>5)
         {
             motor2=0.2  ; 
@@ -254,42 +258,40 @@
     double PID_controller_motor1(double &error_integral_motor1, double &error1_prev_motor1)
     {
         //Proportional part
-        kp_motor1 = 1 ;      // moet nog getweaked worden
+        kp_motor1 = 4.9801 ;      // moet nog getweaked worden
         Up_motor1 = kp_motor1 * error1_motor1;
     
         //Integral part
-        Ki_motor1 = 0.1;     // moet nog getweaked worden
+        Ki_motor1 = 22.6073;     // moet nog getweaked worden
         error_integral_motor1 = error_integral_motor1 + (Ts*error1_motor1);       // integrale fout + (de sample tijd * fout)
         Ui_motor1 = Ki_motor1 * error_integral_motor1;                     // (fout * integrale fout)
     
         //Derivative part 
-        Kd_motor1 = 0.1 ;// moet nog getweaked worden  
+        Kd_motor1 = 0.012757 ;// moet nog getweaked worden  
         error1_derivative_motor1 = (error1_motor1-error1_prev_motor1)/Ts; // (Fout - de vorige fout) / tijdstap = afgeleide
         error1_derivative_filtered_motor1 = LowPassFilter_motor1.step(error1_derivative_motor1); //derivative wordt gefiltered
         Ud_motor1 = Kd_motor1 * error1_derivative_filtered_motor1;           // (afgeleide gain) * (afgeleide gefilterde fout) 
         error1_prev_motor1 = error1_motor1;
 
-        P_motor1 = Up_motor1 + Ui_motor1 + Ud_motor1;                                           //sommatie van de u's
+        P_motor1 = Up_motor1 ;//+ Ui_motor1 + Ud_motor1;                                           //sommatie van de u's
         
         return P_motor1; 
-        
-
     }
 
     // 4.2b PID-Controller motor 2**************************************************
     double PID_controller_motor2(double &error_integral_motor2, double &error1_prev_motor2)
     {
         //Proportional part
-        kp_motor2 = 1 ;      // moet nog getweaked worden
+        kp_motor2 = 4.9801 ;      // moet nog getweaked worden
         Up_motor2 = kp_motor2 * error1_motor2;
     
         //Integral part
-        Ki_motor2 = 0.1;     // moet nog getweaked worden
+        Ki_motor2 = 22.6073;     // moet nog getweaked worden
         error_integral_motor2 = error_integral_motor2 + (Ts*error1_motor2);       // integrale fout + (de sample tijd * fout)
         Ui_motor2 = Ki_motor2 * error_integral_motor2;                     //de fout keer de integrale fout
     
         //Derivative part 
-        Kd_motor2 = 0.1 ;// moet nog getweaked worden  
+        Kd_motor2 = 0.012757 ;// moet nog getweaked worden  
         error1_derivative_motor2 = (error1_motor2  -  error1_prev_motor2)/Ts;
         error1_derivative_filtered_motor2 = LowPassFilter_motor2.step(error1_derivative_motor2); //derivative wordt gefiltered, dit later aanpassen
         Ud_motor2 = Kd_motor2 * error1_derivative_filtered_motor2;
@@ -326,7 +328,7 @@
         
         if (P_motor2 >=0 ) // Als de stuursignaal groter is als 0, dan clockwise rotatie, anders counterclockwise rotatie
         {                
-            motor2DirectionPin=2;   // Clockwise rotation
+            motor2DirectionPin=1;   // Clockwise rotation
         } 
         else 
         {
@@ -410,7 +412,7 @@
     
     double Angle_motor1()
     {
-        a = atan(y / x);
+        a = atan2(y , x);
         b = asin((length_link2 * sin(theta_motor2)) / (sqrt(pow(x, 2.0) + pow(y, 2.0))));
         theta_motor1 = b + a;
         return theta_motor1;
@@ -427,22 +429,30 @@
     double RKI()
     {
         if (emg0_active==true)
-        { if (x>x_max) {x=x_max;}
-          else { x=x+ x_vergroting ; led_blue.write(0); }  
+        { 
+         led1=1;
+          if (x>x_max) {x=x_max;}
+          else { x=x+ x_vergroting ;  }  
         }
         
         if (emg1_active==true)
-        { if (y>y_max) {y=y_max;}
-          else {y=y + y_vergroting; }
+        { 
+          led2=1;
+          if (y>y_max) {y=y_max;}
+          else {y=y + y_vergroting ;  }
          }
         
         if (emg2_active==true)
-        { if (x<x_min) {x=x_min;}
+        { 
+         led1=0;
+        if (x<x_min) {x=x_min;}
           else {x = x - x_vergroting; } 
         }
         
         if (emg3_active==true)
-        { if (y<y_min) {y=y_min;}
+        {
+         led2=1;
+          if (y<y_min) {y=y_min;}
          else {y=y - y_vergroting;} 
         } 
         
@@ -453,9 +463,10 @@
     void motor1_controller(void)
     {
         error1_motor1 = (theta_motor1 - positie_motor1);
+         
         if (motor1_calibrated==true&&motor2_calibrated==true)
             {
-                motor1_pwm();
+               motor1_pwm();
             } 
         
     }
@@ -478,13 +489,15 @@
     {       
         case MotorCalibration: 
 //            pc.printf("\r\n State: MotorCalibration");
-            led_blue.write(1);
+            led_blue.write(0);
             led_red.write(1);
-            led_green.write(0); //Green Led on when in this state
+            led_green.write(1); //Green Led on when in this state
             
             if (motor1_calibrated==true&&motor2_calibrated==true)
             {
                 pc.printf("\r\n Motor Calibration is done!");
+               // x=28; aangeven wat de huidige positie is 
+               // y=11;
                 encoder_motor1.reset();
                 encoder_motor2.reset();
 
@@ -499,10 +512,10 @@
  //           pc.printf("\r\n State: StartWait Button 1 = operation, Button 2 = Demo"); 
             led_blue.write(0);
             led_red.write(1);
-            led_green.write(1);
+            led_green.write(0);
             
             emg_tijd =0;
-            calibration_done=true;
+            calibration_done=false;
             State=EMGCalibration;
             break;  
                        
@@ -514,10 +527,10 @@
             
             emg_tijd++;
             
-            if (0<=emg_tijd&&emg_tijd<=5000)
-            { led_green.write(0); }
-            else if (5000<emg_tijd&&emg_tijd<8000)
-            {   led_green.write(1);
+            if (0<=emg_tijd&&emg_tijd<=3000)
+            { led_green.write(1); }
+            else if (3000<emg_tijd&&emg_tijd<6000)
+            {   led_green.write(0);
                 if (emg0_signal > emg0_max)
                 {
                 emg0_max=emg0_signal;
@@ -538,8 +551,9 @@
                 emg3_max=emg3_signal;
                 }         
             }
-            else if (8000<emg_tijd)
-            {led_green.write(0);
+            else if (6000<emg_tijd)
+            {led_green.write(1);
+            
                calibration_done=true; // later verplaatsen
                 if(button1==0) {State=StartWait;}
                 if(button2==0) {State=Homing;}
@@ -558,8 +572,9 @@
             y= y_home;
             if (homing_tijd>1000) //Voorkomt dat het automatisch naar Demo springt, omdat je bij de vorige nog knop 2 hebt ingedrukt
             {
-                if(button1==0) {State=Operating;}
-                if(button2==0) {State=Demo;} 
+                if(button1==0) {State=Demo;} 
+                if(button2==0) {State=Operating;}
+                
             }
             
             break; 
@@ -573,10 +588,32 @@
             break; 
         
         case Demo:
-           pc.printf("\r\n State: Demo");
+          // pc.printf("\r\n State: Demo");
             led_blue.write(1);
-            led_red.write(1);
-            led_green.write(0); 
+            led_red.write(0);
+            led_green.write(1); 
+            
+            motor1_calibrated=true;
+            motor2_calibrated=true;
+            emg0_threshhold=100;
+            emg1_threshhold=100;
+            emg2_threshhold=100;
+            emg3_threshhold=100;
+            
+            
+            if (button1==0)
+            {    led1=1;
+                if (y>y_max) {y=y_max;}
+                else { y=y+ y_vergroting ;}
+            }
+            if (button2==0)
+            {   led1=0;
+                if (y<y_min) {y=y_min;}
+                else { y=y - y_vergroting ;}
+            }
+            
+            
+            x = 30*pot2_rechts.read();
 
             break;
             
@@ -639,7 +676,7 @@
 // 5.2 Run state-machine(s) ****************************************************
 state_machine() ;
 // 5.3 Run controller(s) *******************************************************
-motor1_calibrated=true;motor2_calibrated=true;
+
 RKI();
 
 PID_controller_motor1(error_integral_motor1, error1_prev_motor1);
@@ -668,14 +705,15 @@
     led_green.write(1);
     led_red.write(1);
     led_blue.write(1);
-    State = EMGCalibration ; // veranderen naar MotorCalibration;
+    State = Demo ; // veranderen naar MotorCalibration;
     ticker_hidscope.attach(&HIDScope, 0.001); //Ticker for Hidscope, different frequency compared to motors
     ticker_mainloop.attach(&main_loop,0.001); // change back to 0.001f //Run the function main_loop 1000 times per second       
     
-   // motor_calibration();
+   motor_calibration();
     
     
 // 6.2 While loop in main function**********************************************   
-    while (true) { pc.printf("\r\n y = %f, theta_motor2 = %f, error1_motor2 = %f", y, theta_motor2, error1_motor2);} //Is not used but has to remain in the code!!
+    while (true) { wait(0.5); pc.printf("\r\n x = %f, y = %f, \r\n theta_motor1 = %f, theta_motor2 = %f, error1_motor1 = %f", x,y, theta_motor1, theta_motor2, error1_motor1);
+                    pc.printf("\r\n emg0 = %f, emg1 = %f, emg2 = %f, emg3 = %f", emg0_signal, emg1_signal, emg2_signal, emg3_signal);} //Is not used but has to remain in the code!!
     
 }                  //Ending of Main() Function