Diff: main.cpp

Revision:

0:df93928b266c

diff -r 000000000000 -r df93928b266c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 30 13:53:06 2017 +0000
@@ -0,0 +1,291 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "PID.h"
+#include "Motor.h"
+#include "BrocketJacobian.h"
+#include "ReceiverIR.h"
+#include "Servo.h"
+#include "emg.h"
+
+// Declare constants etc
+float Interval=0.02f;
+float pi=3.14159265359;
+
+// Declare Analogin in for Potmeter, Can be used for references.
+AnalogIn PotMeter1(A0);
+AnalogIn PotMeter2(A1);
+
+// Declare IR receiver
+ReceiverIR ir_rx(D2);
+ 
+// 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);
+bool EMG_Direction = 0;
+InterruptIn DirectionButton(D6); // Switch 2
+
+// Declare tickers
+Ticker ControlTicker;
+Ticker GetRefTicker;
+ 
+// 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 
+ 
+// 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;
+ 
+// Declare stuff for the IR receiver
+RemoteIR::Format format;
+uint8_t buf[4];
+
+// Declare serial object for setting the baudrate
+RawSerial pc(USBTX,USBRX);
+
+void DirectionButtonPressed(){
+    EMG_Direction=!EMG_Direction;
+}
+
+// Looping function using the Jacobian transposed
+void LoopFunctionJacTransposed()
+{
+    float q1 = Motor1.GetPos();
+    float q2 = Motor2.GetPos();
+    
+    //start values
+    float tau1 = 0.0f;             //previous values are irrelevant
+    float tau2 = 0.0f;             //previous values are irrelevant
+ 
+    float Xcurr = 0.0f;            //new value is calculated, old replaced
+    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
+    
+    // Call function to calculate Xcurr and Ycurr from q1 and q2
+    Brocket(q1, q2, Xcurr, Ycurr);
+        
+    // Compute spring forces 
+    float Fx = k*(GXset-Xcurr);
+    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); 
+    
+    // torque to joint velocity
+    float omg1 = tau1/b;
+    float omg2 = tau2/b;
+
+    // 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;
+    
+    // Call the function that controlls the motors
+    Motor1.GotoPos(q1set);
+    Motor2.GotoPos(q2set);
+}
+
+
+void LoopJacInverse(){
+    // Get Motor Positions
+    float q1 = Motor1.GetPos();
+    float q2 = Motor2.GetPos();
+    
+    // Define velocities for control
+    float q1_dot=0.0f;
+    float q2_dot=0.0f;
+    
+    // Get the velocities from EMG
+    float vx=0.0f;
+    float vy=0.0f;
+    
+    // Apply Jacobian Inverse
+    InverseJacobian(q1,q2,vx,vy,q1_dot,q2_dot);
+    
+    // Integrate q1dot and q2dot to obtain positions
+    q1set += q1_dot*Interval;
+    q2set += q2_dot*Interval;
+    
+    // Call the motor control functions
+    Motor1.GotoPos(q1set);
+    Motor2.GotoPos(q2set);
+}
+ 
+ 
+// 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));
+}
+
+// Function for picking up a checkers playthingy
+void PickUp(){
+ 
+}
+
+// Function for dropping a checkers playthingy
+void LayDown(){
+    
+}
+
+// Forward declarate remote controller function
+void RemoteController();
+ 
+ 
+// Give Reference Position
+void DeterminePosRef(){
+    GXset=40*PotMeter1.read(); // Reference in Rad
+    GYset=40*PotMeter2.read(); // Reference in Rad
+}
+ 
+int main() {
+    pc.baud(115200);
+    pc.printf("Program BIOROBOTICS startup\r\n");
+    
+    // Define Controller Values
+    Motor1.SetInputLimits(-2.0f*pi,2.0f*pi);
+    Motor1.SetInputLimits(-2.0f*pi,2.0f*pi);
+    
+    Motor2.SetOutputLimits(-0.15f,0.15f);
+    Motor2.SetOutputLimits(-0.5f,0.5f);
+    
+    Motor1.SetPID(100.0f,0.0f,0.001f);
+    Motor2.SetPID(100.0f,0.0f,0.001f);
+    
+    Motor1.SetGearRatio(3.0f);
+    Motor2.SetGearRatio(1.8f);
+  
+    // Start homing function
+    pc.printf("Homing \r\n");
+    HomingLoop();
+    
+    // Start Tickers
+    pc.printf("Starting Tickers \r\n");
+    ControlTicker.attach(&LoopFunctionJacTransposed,Interval);
+    GetRefTicker.attach(&DeterminePosRef,0.5f);
+    DirectionButton.rise(&DirectionButtonPressed);
+ 
+    // Check wheater a remote command has been send
+    while(1)
+    { 
+        if (ir_rx.getState() == ReceiverIR::Received)
+        {
+            pc.printf("received");
+            
+            
+            RemoteController();
+            wait(0.1);
+        }
+    }       
+    
+    
+    while(1){}
+ 
+ 
+}
+
+
+ 
+void RemoteController(){
+    
+   int bitcount;
+ 
+ 
+   bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
+    int state = buf[2];
+    switch(state)
+    {
+        case 22: //1
+            pc.printf("1\n\r");
+        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);
+            break;
+        case 94: //1
+            pc.printf("6\n\r");
+            break;
+        case 8: //1
+            pc.printf("7\n\r");
+            break;
+        case 28: //1
+            pc.printf("8\n\r");
+            ControlTicker.detach();
+            Motor1.Stop();
+            Motor2.Stop();
+            LayDown();
+            //ControlTicker.attach(&ControlLoop, 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);
+            break;
+        case 21: //1
+            pc.printf("Onder\n\r");
+            //PosRef2=PosRef2+0.1f;  
+            /*
+            GYset = GYset - 1;
+            */
+            //pc.printf("%f\n\r", PosRef2);
+            break;
+        case 68: //1
+            pc.printf("Links\n\r");
+            //PosRef1=PosRef1+0.1f;
+            /*
+            GXset = GXset + 1;
+            */
+            //pc.printf("%f\n\r", PosRef1);
+            break;
+        case 67: //1
+            pc.printf("Rechts\n\r");
+            //PosRef1=PosRef1-0.1f;
+            /*
+            GXset = GXset - 1;
+            */
+            //pc.printf("%f\n\r", PosRef1);
+            break;
+        case 64: //1
+            pc.printf("OK\n\r");
+            //ControlTicker.detach();
+            //MotorThrottle1=0.0f;
+            //MotorThrottle2=0.0f;
+            //HomingLoop();
+            //ControlTicker.attach(&ControlLoop, Interval);
+            
+            break;
+        default:
+            break;
+    }
+}
\ No newline at end of file