Revision 6:643c44fb044a, committed 2017-11-03

Comitter:

RoyvZ

Date:

Fri Nov 03 10:56:44 2017 +0000

Parent:

5:3562c205d001

Commit message:

Tried

Changed in this revision

diff -r 3562c205d001 -r 643c44fb044a main.cpp
--- a/main.cpp	Fri Nov 03 06:40:08 2017 +0000
+++ b/main.cpp	Fri Nov 03 10:56:44 2017 +0000
@@ -22,6 +22,7 @@
 DigitalIn               changeX(D2);            
 DigitalIn               changeY(D3);
 InterruptIn             turning(D10);
+DigitalIn               buttonMove(PTC6);
 
 Ticker                  sample_timer;
 Ticker                  calX_timer;
@@ -168,7 +169,7 @@
     SX.push_back(emgXvalue);                             //add extra value to begin of vector
     
     SY.erase(SY.begin());                         //remove first value
-    double emgYvalue = bqc.step(emg0.read()-0.4542f);
+    double emgYvalue = bqc.step(emg1.read()-0.4542f);
     SY.push_back(emgYvalue);                             //add extra value to begin of vector
     
     
@@ -249,8 +250,8 @@
     // Fill Matrix with data.
     int Xchanger = (changeX == 1) ? -1 : 1;
     int Ychanger = (changeY == 1) ? -1 : 1;
-    Vdes(1,1) = emgX*Xchanger*NoTurnies;
-    Vdes(2,1) = emgY*Ychanger*NoTurnies;
+    Vdes(1,1) = (emgX-0.1f);//*Xchanger*NoTurnies;
+    Vdes(2,1) = (emgY-0.1f);//*Ychanger*NoTurnies;
     int turnChanger = (turning == 1) ? -1 : 1;
     
     qdot = JAPPAPP*Vdes;
@@ -404,12 +405,13 @@
 //StateMachine for the moving of the end effector
 
 void ProcessStateMachineMain(void) {
-    states CurrentState1 = calibEMG;
+    states CurrentState1 = MotionEndEffector;
     switch (CurrentState1)
         {        
         case MotionEndEffector:
+            setpoint_Ticker.attach(&qSetpointSet, Periode);
             m1_Ticker.attach( &PositionControl, M1_TS );    
-            setpoint_Ticker.attach(&qSetpointSet, Periode);  
+  
         break;
         
         case calibEMG:
@@ -427,24 +429,25 @@
             UpperMotorVel = Cal_Strength; UpperMotorDir = true;          //depends on definitions
             LowerMotorVel = 0.0f;
             UpperMotorPos = encoder1.getPosition();
-            if (abs(UpperMotorPos - UpperMotorPos_prev) < 0.01f){
-                if (Cal_Strength < 1.0f) { 
+            if (abs(UpperMotorPos - UpperMotorPos_prev) < 100){
+                if (Cal_Strength < 0.7f) { 
                     Cal_Strength += 0.05f;
                 }
                 else{
-                    encoder1.setPosition(0);            //Afhankelijk van RKI uitzoeken of deze echt naar nul moet of naar 0.5pi
-                    CurrentState1 = calib2;
-                    Cal_Strength = 0.6f;
+                encoder1.setPosition(0);            //Afhankelijk van RKI uitzoeken of deze echt naar nul moet of naar 0.5pi
+                CurrentState1 = calib2;
+                Cal_Strength = 0.6f;
                 }
-            int UpperMotorPos_prev = UpperMotorPos;
-            } 
+                UpperMotorPos_prev = UpperMotorPos;
+            }
+             
         break;
 
         case calib2:
             LowerMotorVel = Cal_Strength; LowerMotorDir = true;          // Check definitions
-            UpperMotorVel = 1.0f;
+            UpperMotorVel = 0.3f; UpperMotorDir = true;
             LowerMotorPos = encoder2.getPosition();
-            if (abs(LowerMotorPos - UpperMotorPos_prev) < 0.01f){
+            if (abs(LowerMotorPos - LowerMotorPos_prev) < 10){
                 if (Cal_Strength < 1.0f) { 
                     Cal_Strength += 0.05f;
                 }
@@ -454,7 +457,7 @@
                     //M1_controller = true; //turn on the position controller here
                     UpperMotorVel = 0.0f; LowerMotorVel = 0.0f;
                 }
-                int UpperMotorPos_prev = UpperMotorPos;
+                LowerMotorPos_prev = LowerMotorPos;
             }
         break;
        
@@ -478,9 +481,9 @@
 //Main function for the servo motors, to change open or closed state
 int main() {
     //allow for the 2000 Hz sampling of the Servo state machine function
-    MainTicker.attach(ProcessStateMachineMain, 0.0005);
+    //MainTicker.attach(ProcessStateMachineMain, 0.0005);
     
-    ServoTick.attach(ProcessStateMachine, 0.0005);
+    //ServoTick.attach(ProcessStateMachine, 0.0005);
     
       
             
@@ -501,20 +504,27 @@
     
     //MODSERIAL connection
     pc.baud(115200);
+     setpoint_Ticker.attach(&qSetpointSet, Periode);
+     m1_Ticker.attach( &PositionControl, M1_TS );  
     
     
     while (true)
     {
-
+        /*
         if (buttonCal.read() == false){   //when calibrate button is pressed
             states CurrentState1 = calibEMG;
         }  
         
+        if (buttonMove.read() == false){
+            states CurrentState1 = MotionEndEffector;
+        }
+        */
+        /*
         if (!ServoButton) {
             Change();
             wait(0.3);
         }
-        
+        */
         
         /*Control end-effector
         wait(0.1);