Diff: main.cpp

Revision:

1:d4fa7e5e42cc

Parent:

0:df93928b266c

--- a/main.cpp	Mon Oct 30 13:53:06 2017 +0000
+++ b/main.cpp	Wed Nov 01 19:25:02 2017 +0000
@@ -6,9 +6,10 @@
 #include "ReceiverIR.h"
 #include "Servo.h"
 #include "emg.h"
+#include "filter.h"
 
 // Declare constants etc
-float Interval=0.02f;
+float Interval=0.01f;   
 float pi=3.14159265359;
 
 // Declare Analogin in for Potmeter, Can be used for references.
@@ -18,38 +19,48 @@
 // Declare IR receiver
 ReceiverIR ir_rx(D2);
  
+// Declare Servo
+//Servo ZServo(A5);
+PwmOut ZServo(A5);
+// Declare Magnet
+DigitalOut Magnet(D0);
+ 
 // Declare motor objects that will control the motor
 Motor Motor1(D5,D4,D12,D13,D9,Interval);
 Motor Motor2(D6,D7,D10,D11,D8,Interval);
 
 // Declare EMG shields and variables
-emg_shield emg1(A0,500);
-emg_shield emg2(A1,500);
-emg_shield emg3(A2,500);
+emg_shield emg1(A2,500);
+emg_shield emg2(A3,500);
+emg_shield emg3(A4,500);
 bool EMG_Direction = 0;
-InterruptIn DirectionButton(D6); // Switch 2
+InterruptIn DirectionButton(NC); // Switch 2
 
 // Declare tickers
 Ticker ControlTicker;
 Ticker GetRefTicker;
+Ticker GotoPosTicker;
  
 // Delare the GYset and GXset, which are the positions derived from integrating
 // after the applying the jacobian inverse
 float GXset = 40.5;    //from EMG in cm
 float GYset = 11;    //from EMG in cm 
+
+// Motor angles in radialen
+float q1set = 0.29f+0.1f;
+float q2set = (3.3715f-2.0f*pi)+0.1f;
  
-// Constant robot parameters
-const float L1 = 27.5f;      //length arm 1 in cm
-const float L2 = 32.0f;        //length arm 2 in cm
- 
-// Motor angles in radialen
-float q1set = 0.25f*pi;
-float q2set = -0.5f*pi;
+float x_dot_remote = 0.0f;
+float y_dot_remote = 0.0f;
  
 // Declare stuff for the IR receiver
 RemoteIR::Format format;
 uint8_t buf[4];
 
+
+bool EMG_CONTROL=0;
+
+
 // Declare serial object for setting the baudrate
 RawSerial pc(USBTX,USBRX);
 
@@ -71,7 +82,7 @@
     float Ycurr = 0.0f;            //new value is calculated, old replaced
     
     float b = 200.0f;          //damping
-    float k = 0.05f;         //stiffness of the spring pulling on the end effector
+    float k = 1.0f;         //stiffness of the spring pulling on the end effector
     
     // Call function to calculate Xcurr and Ycurr from q1 and q2
     Brocket(q1, q2, Xcurr, Ycurr);
@@ -81,21 +92,54 @@
     float Fy = k*(GYset-Ycurr);
     
     // Call function to calculate tau1 and tau2 from X,Ycurr and X,Yset
-    InverseJacobian(q1, q2, Fx, Fy, tau1, tau2); 
+    TransposeJacobian(q1, q2, Fx, Fy, tau1, tau2); 
     
     // torque to joint velocity
     float omg1 = tau1/b;
     float omg2 = tau2/b;
+    
+    printf("%f %f\r\n",GXset,GYset);
 
     // joint velocity to angles q1 and q2, where you define new q1 and q2 based on previous q1 and q2
     q1set = q1set + omg1*Interval;
     q2set = q2set + omg2*Interval;
+    if (q1set>(0.75f*pi))   {q1set=(0.75f*pi);}
+    if (q1set<0.30f)        {q1set = 0.30f;}
+    if (q2set>0.0f)         {q2set=(0.0f);}
+    if (q2set<-2.79f)        {q2set = -2.79f;}
     
     // Call the function that controlls the motors
     Motor1.GotoPos(q1set);
     Motor2.GotoPos(q2set);
+    //printf("%f %f\r\n",q1set,q2set);
 }
 
+void GotoPosition(float x, float y){
+    float error =10.0f;
+    LowPass RefLowPass1;
+    LowPass RefLowPass2;
+    RefLowPass1.a=0.99;
+    RefLowPass2.a=0.99;
+    
+    float Xcurr; float Ycurr;
+    Brocket(Motor1.GetPos(), Motor2.GetPos(), Xcurr, Ycurr);
+    
+    RefLowPass1.yprev=Xcurr;
+    RefLowPass2.yprev=Ycurr;
+    
+    GXset=RefLowPass1.filter(x);
+    GYset=RefLowPass2.filter(y);
+    
+    GotoPosTicker.attach(&LoopFunctionJacTransposed,Interval);
+    while (error>5.0f){
+        GXset=RefLowPass1.filter(x);
+        GYset=RefLowPass2.filter(y);
+        Brocket(Motor1.GetPos(), Motor2.GetPos(), Xcurr, Ycurr);
+        error = sqrt(pow(Xcurr-x,2)+pow(Ycurr-y,2));
+        wait(0.1);
+    }
+    GotoPosTicker.detach();
+}
 
 void LoopJacInverse(){
     // Get Motor Positions
@@ -106,10 +150,24 @@
     float q1_dot=0.0f;
     float q2_dot=0.0f;
     
-    // Get the velocities from EMG
     float vx=0.0f;
     float vy=0.0f;
     
+    if (EMG_CONTROL){
+        // Get the velocities from EMG
+        float ScaleFactor = 200.0f;
+        vx=ScaleFactor*(emg1.GetValue()- 2.0f*emg2.GetValue());
+        vy=0.0f;//sclaeFactor*(emg2.GetValue());
+    }
+    
+    
+    if ( !(x_dot_remote ==0.0f)){
+        vx=x_dot_remote;
+    }
+    if ( !(y_dot_remote == 0.0f)){
+        vy=y_dot_remote;
+    }
+    
     // Apply Jacobian Inverse
     InverseJacobian(q1,q2,vx,vy,q1_dot,q2_dot);
     
@@ -117,6 +175,11 @@
     q1set += q1_dot*Interval;
     q2set += q2_dot*Interval;
     
+    if (q1set>(0.75f*pi))   {q1set=(0.75f*pi);}
+    if (q1set<0.30f)        {q1set = 0.30f;}
+    if (q2set>0.0f)         {q2set=(0.0f);}
+    if (q2set<-2.79f)        {q2set = -2.79f;}
+    
     // Call the motor control functions
     Motor1.GotoPos(q1set);
     Motor2.GotoPos(q2set);
@@ -126,18 +189,30 @@
 // Start homing the motors
 void HomingLoop(){
     // with param:(Direction , PWM , Home pos in radians)
-    Motor1.Homing(1,0.2f,0.29f);
-    Motor2.Homing(1,0.1f,(3.3715f-2.0f*pi));
+    ZServo.write(0.1f);       // Varying the servo pwm between 2.5 and 10 % sends it between 0 and 180 degrees
+    wait(1);
+    Motor1.Homing(1,0.1f,0.2879f);
+    Motor2.Homing(1,0.15f,-2.897f);
 }
 
 // Function for picking up a checkers playthingy
 void PickUp(){
- 
+    ZServo.write(0.035f);       // Varying the servo pwm between 2.5 and 10 % sends it between 0 and 180 degrees
+    wait(1);
+    Magnet=1;
+    ZServo.write(0.1f);
+    wait(1);
 }
 
 // Function for dropping a checkers playthingy
 void LayDown(){
-    
+    //ZServo = 0.0f;
+    ZServo.write(0.035f);       // Varying the servo pwm between 2.5 and 10 % sends it between 0 and 180 degrees
+    wait(1);
+    Magnet=0;
+
+    ZServo.write(0.1f);
+    wait(1);
 }
 
 // Forward declarate remote controller function
@@ -146,23 +221,28 @@
  
 // Give Reference Position
 void DeterminePosRef(){
-    GXset=40*PotMeter1.read(); // Reference in Rad
-    GYset=40*PotMeter2.read(); // Reference in Rad
+    //GXset=40*PotMeter1.read(); // Reference in Rad
+    //GYset=40*PotMeter2.read(); // Reference in Rad
+    //printf("Set: %f %f",GXset,GYset);
 }
  
 int main() {
+    //ZServo.calibrate(0.0005,120);
+    ZServo.period_ms(20);               // Servo pwm interval should be 50 Hz
     pc.baud(115200);
     pc.printf("Program BIOROBOTICS startup\r\n");
+    q1set = 0.25f*pi;
+    q2set = -0.75f*pi;
     
     // Define Controller Values
     Motor1.SetInputLimits(-2.0f*pi,2.0f*pi);
-    Motor1.SetInputLimits(-2.0f*pi,2.0f*pi);
+    Motor2.SetInputLimits(-2.0f*pi,2.0f*pi);
     
-    Motor2.SetOutputLimits(-0.15f,0.15f);
-    Motor2.SetOutputLimits(-0.5f,0.5f);
+    Motor1.SetOutputLimits(-0.3f,0.3f);
+    Motor2.SetOutputLimits(-0.8f,0.8f);
     
-    Motor1.SetPID(100.0f,0.0f,0.001f);
-    Motor2.SetPID(100.0f,0.0f,0.001f);
+    Motor1.SetPID(50.0f,10.0f,0.001f);
+    Motor2.SetPID(50.0f,10.0f,0.001f);
     
     Motor1.SetGearRatio(3.0f);
     Motor2.SetGearRatio(1.8f);
@@ -173,8 +253,7 @@
     
     // Start Tickers
     pc.printf("Starting Tickers \r\n");
-    ControlTicker.attach(&LoopFunctionJacTransposed,Interval);
-    GetRefTicker.attach(&DeterminePosRef,0.5f);
+    ControlTicker.attach(&LoopJacInverse,Interval);
     DirectionButton.rise(&DirectionButtonPressed);
  
     // Check wheater a remote command has been send
@@ -209,26 +288,39 @@
     {
         case 22: //1
             pc.printf("1\n\r");
-        break;
+            ControlTicker.attach(&LoopJacInverse,Interval);
+            break;
+            
         case 25: //1
             pc.printf("2\n\r");
-            break;
-        case 13: //1                
-            pc.printf("3\n\r");
-            break;
-        case 12: //1
-            pc.printf("4\n\r");
-            break;
-        case 24: //1
-            pc.printf("5\n\r");
             ControlTicker.detach();
             Motor1.Stop();
             Motor2.Stop();
             PickUp();
-            //ControlTicker.attach(&ControlLoop, Interval);
+            ControlTicker.attach(&LoopJacInverse,Interval);
+            break;
+            
+        case 13: //1                
+            pc.printf("3\n\r");
+            
+            break;
+            
+        case 12: //1
+            pc.printf("4\n\r");
+            ControlTicker.detach();
+            Motor1.Stop();
+            Motor2.Stop();
+            GotoPosition(0.0f,0.0f);
+            break;
+            
+        case 24: //1
+            pc.printf("5\n\r");
             break;
         case 94: //1
             pc.printf("6\n\r");
+            ControlTicker.detach();
+            Motor1.Stop();
+            Motor2.Stop();
             break;
         case 8: //1
             pc.printf("7\n\r");
@@ -239,51 +331,31 @@
             Motor1.Stop();
             Motor2.Stop();
             LayDown();
-            //ControlTicker.attach(&ControlLoop, Interval);
+            ControlTicker.attach(&LoopJacInverse,Interval);
             break;
         case 90: //1
             pc.printf("9\n\r");
             break;
         case 70: //1
             pc.printf("Boven\n\r");
-            //PosRef2=PosRef2-0.1f;
-            /*
-            GYset = GYset + 1;
-            */
-            //pc.printf("%f\n\r", PosRef2);
+            y_dot_remote = 5.0f;
             break;
         case 21: //1
             pc.printf("Onder\n\r");
-            //PosRef2=PosRef2+0.1f;  
-            /*
-            GYset = GYset - 1;
-            */
-            //pc.printf("%f\n\r", PosRef2);
+            y_dot_remote = -5.0f;
             break;
         case 68: //1
             pc.printf("Links\n\r");
-            //PosRef1=PosRef1+0.1f;
-            /*
-            GXset = GXset + 1;
-            */
-            //pc.printf("%f\n\r", PosRef1);
+            x_dot_remote = -5.0f;
             break;
         case 67: //1
             pc.printf("Rechts\n\r");
-            //PosRef1=PosRef1-0.1f;
-            /*
-            GXset = GXset - 1;
-            */
-            //pc.printf("%f\n\r", PosRef1);
+            x_dot_remote = 5.0f;
             break;
         case 64: //1
             pc.printf("OK\n\r");
-            //ControlTicker.detach();
-            //MotorThrottle1=0.0f;
-            //MotorThrottle2=0.0f;
-            //HomingLoop();
-            //ControlTicker.attach(&ControlLoop, Interval);
-            
+            x_dot_remote = 0.0f;
+            y_dot_remote = 0.0f;
             break;
         default:
             break;