Diff: main.cpp

Revision:

8:83b2ab57cf0d

Parent:

7:1d3eefa00e20

Child:

9:31c8de7ac6fe

--- a/main.cpp	Thu Nov 02 13:37:38 2017 +0000
+++ b/main.cpp	Thu Nov 02 15:22:38 2017 +0000
@@ -3,54 +3,56 @@
 #include "encoder.h"
 #include "MODSERIAL.h"
 
+// globale variables
+Ticker Treecko;                 //We make a awesome ticker for our control system
+const float Ts = 0.1;                   // tickettijd/ sample time
+MODSERIAL pc(USBTX,USBRX);
+float PwmPeriod = 1.0/5000.0;   //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
+AnalogIn potMeter2(A1);         //Analoge input of potmeter 2 (will be use for te reference position)
+AnalogIn potMeter1(A2);
+DigitalIn Knopje1 (PTA4);       //tijdelijk
+DigitalIn Knopje2 (PTC6);       //tijdelijk
+bool autodemo_done = 1;         //automatische demo stand =0
 
-// globale variables
-Ticker AInTicker;           //We make a ticker named AIn (use for HIDScope)
-
-Ticker Treecko;             //We make a awesome ticker for our control system
-AnalogIn potMeter2(A1);     //Analoge input of potmeter 2 (will be use for te reference position)
+//eerste motor
 PwmOut M1E(D6);             //Biorobotics Motor 1 PWM control of the speed 
 DigitalOut M1D(D7);         //Biorobotics Motor 1 diraction control
+Encoder motor1(D13,D12,true);
 
-Encoder motor1(D13,D12,true);
-MODSERIAL pc(USBTX,USBRX);
-
-float PwmPeriod = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
-const float Ts = 0.1;                   // tickettijd/ sample time
 float e_prev = 0; 
 float e_int = 0;
 float error1;
-float PwmPeriod2 = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
+
+//tweede motor
+PwmOut M2E(D5);
+DigitalOut M2D(D4);
+Encoder motor2(D9,D8,true);
+
+
 float e_prev2 = 0; 
 float e_int2 = 0;
 float error2;
 
+//RKI
 double pi = 3.14159265359;
 double SetPx = 0.38;     //Setpoint position x-coordinate from changePosition (EMG dependent)
 double SetPy = 0.30;     //Setpoint position y-coordinate from changePosition (EMG dependent)
-double q1 = 0;          //Reference position q1 from calibration (only the first time)
-double q2 = (pi/2);       //Reference position q2 from calibration (only the first time)
+double q1 = 0;           //Reference position q1 from calibration (only the first time)
+double q2 = (pi/2);      //Reference position q2 from calibration (only the first time): LET OP! DE MOTOR
 const double L1 = 0.30;  //Length arm 1
 const double L2 = 0.38;  //Length arm 2
-double K = 1;           //Spring constant for movement end-joint to setpoint
-double B1 = 1;          //Friction coefficient for motor 1
-double B2 = 1;          //Friction coefficient for motot 2
-double T = 0.02;       //Desired time step
-double Motor1Set;       //Motor1 angle
-double Motor2Set;       //Motor2 angle
+double K = 1;            //Spring constant for movement end-joint to setpoint
+double B1 = 1;           //Friction coefficient for motor 1
+double B2 = 1;           //Friction coefficient for motot 2
+double T = 0.1;         //Desired time step
+double Motor1Set;        //Motor1 angle
+double Motor2Set;        //Motor2 angle
 double p;
 double pp;
 double bb;
 double cc;
 double a;
 double aa;
-bool autodemo_done = 1;      //automatische demo stand =0
-
-//tweede motor
-AnalogIn potMeter1(A2);
-PwmOut M2E(D5);
-DigitalOut M2D(D4);
-Encoder motor2(D9,D8,true);
 
 
 void RKI()
@@ -76,8 +78,10 @@
 
 void SetpointRobot()
 {   
-    double Potmeterwaarde2 = potMeter2.read();
-    double Potmeterwaarde1 = potMeter1.read();
+    //double Potmeterwaarde2 = potMeter2.read();
+    //double Potmeterwaarde1 = potMeter1.read();
+    bool knop1 = Knopje1;
+    bool knop2 = Knopje2;
 
     if (Potmeterwaarde2>0.6) {
         SetPx += 0.001;    // hoe veel verder gaat hij? 1 cm? 10 cm?
@@ -178,7 +182,7 @@
 {
     if (motorValue2 >= 0)
     {
-        M2D = 1;
+        M2D = 1; //PAS OP, DEZE MOET ZO ZIJN!
     }
     else 
     {