Diff: main.cpp

Revision:

8:656b0c493a45

Parent:

7:075ba23f3147

Child:

9:e764cb50d343

--- a/main.cpp	Tue Nov 01 10:15:18 2016 +0000
+++ b/main.cpp	Tue Nov 01 11:18:41 2016 +0000
@@ -144,8 +144,8 @@
  
 
  // Controller gains (motor1−Kp,−Ki,...)
- const double Kpm= 0.1, Kim = 0*0.05;
- double er_int = 0;
+ const double Kpm= 0.1, Kim = 0.005;
+ double e_L_int = 0;
  
     double Controller_L(int direction, double signal, double reference ){
         //This funcition returns a value that will be sent to drive the motor
@@ -155,16 +155,25 @@
         // and -1 for direction means the opposite
         
         //PID controller
-       double Speed_Set = PI( reference-signal*direction,Kpm, Kim, SampleFrequency, er_int );
+       double Speed_Set = PI( reference-signal*direction,Kpm, Kim, SampleFrequency, e_L_int );
     //Determining rotational direction of the motor
-            if ((reference-signal*direction >= 0)){
+           if ((reference-signal*direction >= 0)){
                  M_L_D = 1;
             } else {
-                  M_L_D = 0;
+                 M_L_D = 0;
              }
+            if (fabs(Speed_Set)< 0.4){
+                if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance
+                    return  0;
+                    } else {
             return fabs(Speed_Set);
-        }
-        
+            }
+            } else {
+                return 0.4;
+            
+            }
+            }
+      double e_R_int = 0;   
 double Controller_R(int direction, double signal, double reference ){
         //This funcition returns a value that will be sent to drive the motor
         // - Motor_Direction_Input is the Digital in of the motor
@@ -173,7 +182,7 @@
         // and -1 for direction means the opposite
         
         //PI controller
-       double Speed_Set = PI( reference-signal*direction,Kpm, Kim,  SampleFrequency, er_int);
+       double Speed_Set = PI( reference-signal*direction,Kpm, Kim,  SampleFrequency, e_R_int);
     //Determining rotational direction of the motor
             if ((reference-signal*direction >= 0)){
                  M_R_D = 0;
@@ -289,7 +298,30 @@
     }
     
 }
+//------------------------------------------------------Postition reset----------------------------------
+//After the robot has been set in it's equlibrium state, this data is used to prevent the robot of destroying itself.
 
+void Position_reset(){
+    Position_R =0;
+    Position_L =0;
+    }
+
+//-------------------------------------------------------Limit-------------------------------------
+//calculates the angele of the robot with for its limit, and checks of its limit has been reached
+
+int Limit(){
+    double angle=Position_R-Position_L; //R moves the left arm and right moves the right arm
+    int limit = 0;
+    if (fabs(angle) < -13*2*pi*136/(33*360) ){ //limit angle of -13 degrees translated to the motor
+            limit = 1;
+        } 
+        if (angle > 15*2*pi*136/(33*360) ){ //limit angle of 15 degrees tranlated to the motor
+            limit = 2;
+        }
+      return limit;
+
+        
+    }
 /////////////-----------------------Booting---------------------------------------=
 void Boot(){
   
@@ -310,13 +342,14 @@
     Grap = 1;
     grip1 = 1;
     
-        bqc_L.add( &bq1_L ).add( &bq2_L );
+    bqc_L.add( &bq1_L ).add( &bq2_L );
     bqc_R.add(&bq1_R).add(&bq2_R);
     bqc_Change.add(&bq1_Change).add(&bq2_Change);
     Tick_Sample.attach(Set_Sample_Flag, 1.0/SampleFrequency);    Tick_Controller.attach(Set_controller_Flag, 1.0/SampleFrequency);
     Tick_Movement.attach(Set_movement_Flag, 0.25);
     OnOff.fall(Start_Stop);
     
+    int Maxmin;
     }
 
     
@@ -364,8 +397,24 @@
                    translation = 1;
                    break;
                    }
-               M_L_S = Controller_L(direction_L,Signal,Speed_L);
-               M_R_S = Controller_R(direction_R,Signal,Speed_R);
+               switch (Limit()){ //setting limit
+                   case 0:
+                         M_L_S = Controller_L(direction_L,Signal,Speed_L);
+                         M_R_S = Controller_R(direction_R,Signal,Speed_R);
+                   break;
+                   case 1:
+                        if (Signal < 0){
+                            M_L_S = Controller_L(direction_L,Signal,Speed_L);
+                            M_R_S = Controller_R(direction_R,Signal,Speed_R);
+                        }
+                   break;
+                   case 2:
+                        if (Signal > 0){
+                            M_L_S = Controller_L(direction_L,Signal,Speed_L);
+                            M_R_S = Controller_R(direction_R,Signal,Speed_R);
+                        }
+                   break; 
+                   }
                Controller_Flag = false;
             }
         } else {