Diff: main.cpp

Revision:

5:a1843b698d0d

Parent:

4:8f25ecb74221

Child:

6:e67250b8b100

--- a/main.cpp	Wed Oct 31 17:40:25 2018 +0000
+++ b/main.cpp	Thu Nov 01 12:35:45 2018 +0000
@@ -1,7 +1,6 @@
 #include "mbed.h"
 #include "FastPWM.h"
 #include "QEI.h"
-#include "MODSERIAL.h"
 #include "BiQuad.h"
 #include <iostream>
 #include <string>
@@ -15,15 +14,22 @@
 DigitalIn screwingSwitch(SW3);
 DigitalIn gripDirection(D2);
 DigitalIn screwDirection(D3);
-MODSERIAL pc(USBTX, USBRX);
+
+Serial pc(USBTX, USBRX);
+
 DigitalOut led1(LED1); // Red led
 DigitalOut led2(LED2); // Green led
 DigitalOut led3(LED3); // Blue led
 QEI encoder1(D14, D15, NC, 8400, QEI::X4_ENCODING);
 QEI encoder2(D12, D13, NC, 8400, QEI::X4_ENCODING);
 //QEI encoder3(A4, A5), NC, 4200);
-AnalogIn pot(A0); // Speed knob
-AnalogIn pot2(A1);
+AnalogIn pot(A4); // Speed knob
+AnalogIn pot2(A5);
+
+AnalogIn emg0(A0);
+AnalogIn emg1(A1);
+AnalogIn emg2(A2);
+AnalogIn emg3(A3);
 
 bool stateChanged = true;
 
@@ -64,8 +70,8 @@
 DigitalOut directionpin1(D4); // motor direction
 FastPWM pwmpin2 (D6);
 DigitalOut directionpin2 (D7);
-double setPointX;
-double setPointY;
+double setPointX = 0.218;
+double setPointY = 0.328;
 double qRef1;
 double qRef2;
 double qMeas1;
@@ -74,15 +80,29 @@
 double v; // Global variable for printf
 double Dpulses; // Global variable for printf
 double error1; // Global variable for printf
+double error2; // Global variable for printf
 
 double C[5][5];
 
-double Kp1 = 2;
-double Ki1 = 0.01;
-double Kd1 = 0.01;
-double Kp2 = 0.00001;
-double Ki2 = 0;
-double Kd2 = 0.0002;
+double Kp1 = 7;
+double Ki1 = 0.3;
+double Kd1 = 0.4;
+double Kp2 = 5;
+double Ki2 = 0.3;
+double Kd2 = 0.4;
+
+const int samplepack = 250; //Amount of data points before one cycle completes
+volatile int counter = 0; //Counts the amount of readings this cycle
+volatile double total[4] = {0,0,0,0}; //Total difference between data points and the calibration value this cycle (x+, x-, y+, y-)
+double refvaluebic = 10; //Minimum total value for the motor to move (biceps)
+double refvaluecalf = 50; //Minimum total value for the motor to move (calfs)
+double incr = 0.00005; //increment
+bool moving[4] = {0,0,0,0}; //x+, x-, y+, y-
+
+BiQuad hpf(0.8635,-1.7269,0.8635,-1.9359,0.9394); //5Hz High pass filter
+BiQuad notch(0.9922,-1.6054,0.9922,-1.6054,0.9844); //50 Hz Notch filter
+BiQuad lpf(0.9780,1.9561,0.9780,1.9556,0.9565); //250Hz Low pass filter
+BiQuadChain bqc;
 
 void switchToFailureState ()
 {
@@ -95,6 +115,51 @@
     stateChanged = true;
 }
 
+void measureEMG ()
+{
+    total[0] += abs(bqc.step(emg0.read()));
+    total[1] += abs(bqc.step(emg1.read()));
+    total[2] += abs(bqc.step(emg2.read()));
+    total[3] += abs(bqc.step(emg3.read()));
+    counter++;
+    if (counter >= samplepack){
+        moving[0] = 0;
+        moving[1] = 0;
+        moving[2] = 0;
+        moving[3] = 0;
+        if (total[0] >= refvaluebic){
+            moving[0] = 1;
+        }
+        if (total[1] >= refvaluebic){
+            moving[1] = 1;
+        }
+        if (total[2] >= refvaluecalf){
+            moving[2] = 1;
+        }
+        if (total[3] >= refvaluecalf){
+            moving[3] = 1;
+        }
+        pc.printf("Totals of {x+,x-,y+,y-} are %f, %f, %f, %f \r\n",total[0],total[1],total[2],total[3]);
+        counter = 0; //Reset for next cycle
+        for (int i=0;i<4;i++){ //clear 'total' array
+            total[i] = 0;
+        }
+    }
+    if(moving[0]){
+        setPointX += incr;
+    }
+    if (moving[1]){
+        setPointX -= incr;
+    }
+    if (moving[2]){
+        setPointY += incr;
+    }
+    if (moving[3]){
+        setPointY -= incr;
+    }
+    //pc.printf("Coordinates: (%f,%f)\r\n", setPointX, setPointY);
+}
+
 double measureVelocity (int motor)
 {
     static double lastPulses = 0;
@@ -131,8 +196,8 @@
 {
     double pulses1 = encoder1.getPulses();
     double pulses2 = encoder2.getPulses();
-    qMeas1 = (pulses1+3289.53) * 2 * PI / 8400 ; // Calculate the angle relative to the starting point (8400 pulses per revolution) + offset
-    qMeas2 = (pulses2+70) * 2 * PI / 8400 ;
+    qMeas1 = (pulses1) * 2 * PI / 8400 + 140.7*PI/180 ; // Calculate the angle relative to the starting point (8400 pulses per revolution) + offset
+    qMeas2 = -qMeas1 +3*PI/180 ;
 
 }
 
@@ -171,8 +236,8 @@
 void inverseKinematics ()
 {
     if (currentState == MovingState) {  // Only in the HomingState should the qRef1, qRef2 consistently change
-        double potx = 0.4;//pot1.read()*0.546;
-        double  poty = 0.2;//pot2.read()*0.4;
+        double potx = 0.55;//pot1.read()*0.546;
+        double  poty = 0.01;//pot2.read()*0.4;
 
         double  Pe_set[3][1] {            // defining setpoint location of end effector
             {potx},             //setting xcoord to pot 1
@@ -265,20 +330,20 @@
 //Calculating joint behaviour
 
         double b1 =1;
-        double b2 =10000;
+        double b2 =1;
         //joint friction coefficent
         //double sampleTime = 1/1000;               //Time step to reach the new angle
         double w_s1 = tau_st1/b1;          // angular velocity
         double w_s2 = tau_st2/b2;          // angular velocity
         //checking angle boundaries
-        qRef1 = qRef1 +w_s1*sampleTime;         // calculating new angle of qRef1 in time step sampleTime
+        qRef1 = qRef1 +w_s1*1;         // calculating new angle of qRef1 in time step sampleTime
         if (qRef1 > 2*PI/3) {
             qRef1 = 2*PI/3;
         } else if (qRef1 < PI/6) {
             qRef1= PI/6;
         }
 
-        qRef2 = qRef2 + w_s2*sampleTime;        // calculating new angle of qRef2 in time step sampleTime
+        qRef2 = qRef2 + w_s2*1;        // calculating new angle of qRef2 in time step sampleTime
         if (qRef2 > -PI/4) {
             qRef2 = -PI/4;
         } else if (qRef2 < -PI/2) {
@@ -287,10 +352,12 @@
     }
 }
 void PID_controller() // Put the error trough PID control to make output 'u'
+
 {
+     if (currentState >= HomingState && currentState < FailureState){
      // Should only work when we move the robot to a defined position
-        double error1 = qRef1 - qMeas1;
-        double error2 = qRef2 - qMeas2;
+         error1 = qRef1 - qMeas1;
+        error2 = qRef2 - qMeas2;
 
         static double errorIntegral1 = 0;
         static double errorIntegral2 = 0;
@@ -323,8 +390,9 @@
         // Sum all parsampleTime
         u1 = u_k1 + u_i1 + u_d1;
         u2 = u_k2 + u_i2 + u_d2;
+        
     
-    
+    }
 }
 
 void controlMotor ()   // Control direction and speed
@@ -408,8 +476,8 @@
                 led1 = 0;
                 led2 = 0; // EmisampleTime yellow together
                 //TODO: Set qRef1 and qRef2
-                qRef1 = 70 * PI / 180;
-                qRef2 = -90 * PI / 180;
+                qRef1 = 90 * PI / 180;
+                qRef2 = -qRef1 +0 *PI/180;
                 stateChanged = false;
             }
 
@@ -512,6 +580,7 @@
 void measureAll ()
 {
     measurePosition();
+    measureEMG();
     inverseKinematics();
 }
 
@@ -526,8 +595,9 @@
 
 int main()
 {
+    pc.baud(115200);
     pc.printf("checkpoint 1\n");
-    pc.baud(115200);
+    bqc.add(&hpf).add(&notch).add(&lpf); //Add bqfilters to bqc
     mainTicker.attach(mainLoop, sampleTime);
     failureButton.fall(&switchToFailureState); // When button is pressed FailureState is activated
 
@@ -535,8 +605,8 @@
         //pc.printf("State = %i\n", currentState);
         //int pulses = encoder1.getPulses();
         //pc.printf("pulses = %i\n", pulses);
-        pc.printf("v = %f\n", v);
-        pc.printf("Error1 = %f\n qRef1 = %f\n qMeas1 = %f\n", error1,qRef1,qMeas1);
-        wait(1);
+       // pc.printf("v = %f\n", v);
+        //pc.printf("Error1 = %f Error2 = %f \n qRef1 = %f rQref2 = %f \r\n", error1,error2,qRef1,qRef2);
+        wait(2);
     }
 }
\ No newline at end of file