Diff: main.cpp

Revision:

16:deb42ce3c3a1

Parent:

15:38258e6b6e91

Child:

20:11fe0aa7f111

--- a/main.cpp	Wed Oct 31 18:55:04 2018 +0000
+++ b/main.cpp	Wed Oct 31 20:09:58 2018 +0000
@@ -24,11 +24,11 @@
 double omega4;
 
 //Constants
-const double ll = 200.0;
-const double lu = 170.0;
-const double lb = 10.0;
-const double le = 79.0;
-const double xbase = 340;
+const double ll = 198;
+const double lu = 230;
+const double lb = 50;
+const double le = 79;
+const double xbase = 450-lb;
 const int Length = 10000;                                       //Length of the array for the calibration
 const int Parts = 50;                                           //Mean average filter over 50 values
 
@@ -112,7 +112,7 @@
 
 //Parameters for the state machine
 enum States {Calibration, WorkingMode};                         //Initialize state machine
-States CurrentState = Calibration;                              //Start in the calibration mode
+States CurrentState = WorkingMode;                              //Start in the calibration mode
 bool StateBool = true;                                          //Bool to first go in a state
 bool SwitchStateBool = false;                                   //Bool to switch from calibration to working mode
 
@@ -165,7 +165,7 @@
         yMove = true;                                           //Set y movement to true
     } else {
         greenled = 1;                                           //Otherwise turn the LED off
-        yMove = true;                                           //Otherwise set y movement to false
+        yMove = false;                                           //Otherwise set y movement to false
     }
 }
 
@@ -379,12 +379,17 @@
                    sqrt(pow(-(xbase/ll) + cos(theta1) + cos(0.001 + theta4),2) + pow(sin(theta1) - sin(0.001 + theta4),2)))/2.));
 
     //vanaf hier weer door met lezen!
-    prefx = 1*(!xMove); //sw3, dit is belangrijk! prefx staat voor P_(reference) en het is de snelheid van de endeffector als
+    prefx = 10*(xMove); //sw3, dit is belangrijk! prefx staat voor P_(reference) en het is de snelheid van de endeffector als
     // de button ingedrukt wordt (als emg = boven treshold) is de prefx 1 (da's de rode 1)
-    prefy = 1*(!yMove); //sw2,
+    prefy = 10*(yMove); //sw2,
     inverse(prefx, prefy);
 }
 
+void printvalue()
+{
+    pc.printf("X-value: %f \t Y-value: %f \n\r",xend, yend);           // in teraterm zijn de bovenste twee waardes hoeken, de onderste twee zijn de x en y coordinaat
+
+}
 
 //State Machine
 void StateMachine()
@@ -418,57 +423,15 @@
                 EMGCalibration1_timer.detach();                 //Detach the calibration
                 ReadUseEMG0_timer.attach(&ReadUseEMG0, 0.005);  //Start the use of EMG
                 ReadUseEMG1_timer.attach(&ReadUseEMG1,0.005);   //Start the use of EMG
-                kin.attach(kinematics, 0.01); // roep de ticker aan (
+                kin.attach(kinematics, 1); // roep de ticker aan (
+                simulateval.attach(printvalue, 1);
             }
             blueled = 1;                                        //Set the blue led off
-            pc.printf("First EMG: %f, Second EMG: %f \n\r",EMG0Average,EMG1Average);
+            //pc.printf("First EMG: %f, Second EMG: %f \n\r",EMG0Average,EMG1Average);
             break;
     }
 }
 
-
-
-
-
-
-
-/*In de nieuwe versie hieronder is forward overbodig geworden, sla maar over
-
-Joe, hieronder staan de functies die door de tickers aangeroepen worden
-void forward(){        dit is de ticker die zegt, als button=0, theta 1 wordt groter. dan worden x en y doorgerekend
-                         hieronder moet veranderd worden naar if button1 == 0, x = x+eenbeetje
-                         maar daar moet eerst inverse kinematics voor gebeuren.
-    if (button1 == 0){                          als emg1==voorbij treshold, komt waarschijnlijk in de inverse functie
-        theta1 = PI*(theta1/PI + 0.1);
-     hij is geblokt omdat ik de knop nodig heb
-    default = als x = xbase/2... break, okee dit moet hier niet, maar weet niet waar wel...
-    }
-    else {theta1 = theta1;}
-    hieronder komen de doorrekeningen van de hoeken naar de coordinaten, check de mathematicafile voor de afleiding*/
-
-/*void demomode(){}       //Komt nog...
-
-                   als emg2 == voorbij treshold,
-                    double theta1 -> plus counts (emg*richting)
-                    double theta4 -> plus counts (emg*richting)
-                    reken y door
-
-void flip(){
-    if(button2==0){thetaflip = PI*(thetaflip/PI+0.5);} // button2==0 moet veranderd naar emg3>= treshold
-}suppressed omdat ik button 2 nodig heb...*/
-
-
-// de beweging voor de xcoordinaat!
-
-
-//tot aan hier overslaan
-void printvalue()
-{
-    pc.printf("\n\r %f %f \n\r %f %f", theta4,theta1, xend, yend);           // in teraterm zijn de bovenste twee waardes hoeken, de onderste twee zijn de x en y coordinaat
-
-}
-
-
 int main()
 {
     //Initial conditions
@@ -476,20 +439,15 @@
     theta1 = PI*0.49;
     theta4 = PI*0.49;
     pc.baud(115200);
-    //default = theta1 = theta4 = pi/2,
     greenled = 1;                                               //First turn the LEDs off
     blueled = 1;
     redled = 1;
     filter0.add(&Notch50_0).add(&High0);                        //Make filter chain for the first EMG
     filter1.add(&Notch50_1).add(&High1);                        //Make filter chain for the second EMG
     button.rise(StopProgram);                                   //If the button is pressed, stop program
-    simulateval.attach(printvalue, 1);
-
-    pc.printf("%f", theta1);
+    
     while(true) {
 
         StateMachine();                                                          //Start the state machine
-
-
     }
 }
\ No newline at end of file