Diff: main.cpp

Revision:

3:16df793da2be

Parent:

2:5093b66c9b26

Child:

4:7d0df890e801

--- a/main.cpp	Tue Oct 29 15:48:51 2019 +0000
+++ b/main.cpp	Wed Oct 30 15:42:23 2019 +0000
@@ -7,8 +7,10 @@
 
 const double PI = 3.1415926535897;  
 
-InterruptIn but1(D2);
-InterruptIn but2(D3);
+InterruptIn but1(D1);
+InterruptIn but2(D0);
+DigitalIn butsw2(SW2);
+DigitalIn butsw3(SW3);
 AnalogIn potMeter1(A2);
 AnalogIn potMeter2(A1);
 
@@ -21,7 +23,7 @@
 // SERIAL COMMUNICATION WITH PC.................................................
 Serial pc(USBTX, USBRX);
 
-enum States {s_idle, s_cali_enc, s_demo};
+enum States {s_idle, s_calibratie_encoder, s_demo};
 States state;
 
 // ENCODER .....................................................................
@@ -37,22 +39,38 @@
 int enc2_zero = 0;//encoder calibration
 int enc1_value;
 int enc2_value;
+volatile double Huidigepositie1;
+volatile double Huidigepositie2;
+volatile double motorValue1;
+volatile double motorValue2;
+volatile double refP;
+volatile double refP2;
+volatile double error1;
+volatile double error2;
 bool state_changed = false; 
+volatile bool A=true;
+volatile bool B=true;
 volatile bool but1_pressed = false;
 volatile bool but2_pressed = false;
+volatile bool butsw2_pressed = false;
+volatile bool butsw3_pressed = false;
 volatile bool failure_occurred = false;
 
-void do_nothing()
+const bool clockwise = true;
+volatile bool direction1 = clockwise;
+volatile bool direction2 = clockwise;
+
+void rest()
     //Idle state. Used in the beginning, before the calibration states.  
 {
     if (but1_pressed) {
         state_changed = true;
-        state = s_cali_enc;
+        state = s_calibratie_encoder;
         but1_pressed = false;
     }
 }
 
-void cali_enc()
+void calibratie_encoder()
 {
     if (state_changed) {
         pc.printf("Started encoder calibration\r\n");
@@ -60,6 +78,12 @@
     }
     if (but1_pressed) {
         pc.printf("Encoder has been calibrated \r\n");
+        enc1_value = enc1.getPosition();
+        enc2_value = enc2.getPosition();
+    //enc1_value -= enc1_zero;
+    //enc2_value -= enc2_zero;
+        theta1 = (float)(enc1_value)/(float)(8400)*2*PI;
+        theta2 = (float)(enc2_value)/(float)(8400)*2*PI;
         enc1_zero = enc1_value;
         enc2_zero = enc2_value;
         but1_pressed = false;
@@ -71,23 +95,29 @@
         pc.printf("enc2value: %i\r\n", enc2_value);
         pc.printf("hoek 1: %f\r\n",theta1);
         pc.printf("hoek 2: %f\r\n",theta2);
+        }
         
- 
+        if (but2_pressed) {
+        state_changed = true;
+        state = s_idle;
+        but2_pressed = false;
+        pc.printf("FAILURE!\r\n");
+        state_changed = false;
     }
 }
-    
+
 double GetReferencePosition() 
 {
-    double Potmeterwaarde = potMeter1.read(); //naam moet universeel worden
-    int maxwaarde = 200;                   // = 64x64
+    double Potmeterwaarde = potMeter2.read(); //naam moet universeel worden
+    int maxwaarde = 400;                   // = 64x64
     double refP = Potmeterwaarde*maxwaarde;
     return refP;                            // value between 0 and 4096 
 }
  
 double GetReferencePosition2() 
 {
-    double potmeterwaarde2 = potMeter2.read();
-    int maxwaarde2 = 200;                   // = 64x64
+    double potmeterwaarde2 = potMeter1.read();
+    int maxwaarde2 = 400;                   // = 64x64
     double refP2 = potmeterwaarde2*maxwaarde2;
     return refP2;                            // value between 0 and 4096 
 }
@@ -95,15 +125,14 @@
 double FeedBackControl(double error, double &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
 {
     double kp = 0.0015;                               // kind of scaled.
-    double Proportional= kp*error;
+    double Proportional= kp*error1;
     
     double ki = 0.0001;                           // kind of scaled.
-    e_int = e_int+dt*error;
+    e_int = e_int+dt*error1;
     double Integrator = ki*e_int;
     
-    
-    double motorValue = Proportional + Integrator ;
-    return motorValue;
+    motorValue1 = Proportional + Integrator ;
+    return motorValue1;
 }
  
 double FeedBackControl2(double error2, double &e_int2)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
@@ -115,45 +144,21 @@
     e_int2 = e_int2+dt*error2;
     double Integrator2 = ki2*e_int2;
     
-    
     double motorValue2 = Proportional2 + Integrator2 ;
     return motorValue2;
-    
+}
+
+void SetMotor1(float motorValue1) {
+    // Given motorValue1<=1, writes the velocity to the pwm control.
+    // MotorValues outside range are truncated to within range.
+    motor1_pwm.write(fabs(motorValue1) > 1 ? 1 : fabs(motorValue1));
 }
- 
-void SetMotor1(double motorValue)
-{
-    if (motorValue >= 0) {
-        motor1_direction = 0;
-    }
-    else {
-        motor1_direction = 1;
-    }
- 
-    if  (fabs(motorValue) > 1) {
-        motor1_pwm = 1;                    //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
-    }
-    else {    
-        motor1_pwm = fabs(motorValue);      //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
-    }
-}
- 
-void SetMotor2(double motorValue2)
-{
-    if (motorValue2 >= 0) {
-        motor2_direction = 1;
-    }
-    else {
-        motor2_direction = 0;
-    }
- 
-    if  (fabs(motorValue2) > 1) {
-        motor2_pwm = 1;                    //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
-    }
-    else {    
-        motor2_pwm = fabs(motorValue2);      //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
-    }
-}
+
+void SetMotor2(float motorValue2) {
+    // Given motorValue2<=1, writes the velocity to the pwm control.
+    // MotorValues outside range are truncated to within range.
+    motor2_pwm.write(fabs(motorValue2) > 1 ? 1 : fabs(motorValue2));
+}    
  
 void MeasureAndControl(void)
 {
@@ -161,43 +166,70 @@
     // RKI aanroepen
     //RKI();
     // hier the control of the 1st control system
-    double refP = GetReferencePosition();                    //KOMT UIT RKI
-    double Huidigepositie = enc1.getPosition(); 
-    double error = (refP - Huidigepositie);// make an error
-    double motorValue = FeedBackControl(error, e_int);
-    SetMotor1(motorValue);
+    refP = GetReferencePosition();                    //moet nog met RKI
+    Huidigepositie1 = enc1.getPosition(); 
+    error1 = (refP - Huidigepositie1);// make an error
+    motorValue1 = FeedBackControl(error1, e_int);
+    SetMotor1(motorValue1);
     // hier the control of the 2nd control system
-    double refP2 = GetReferencePosition2(); 
-    double Huidigepositie2 = enc2.getPosition(); 
-    double error2 = (refP2 - Huidigepositie2);// make an error
-    double motorValue2 = FeedBackControl2(error2, e_int2);
+    refP2 = GetReferencePosition2(); 
+    Huidigepositie2 = enc2.getPosition(); 
+    error2 = (refP2 - Huidigepositie2);// make an error
+    motorValue2 = FeedBackControl2(error2, e_int2);
     SetMotor2(motorValue2);
-    pc.printf("refP = %f, huidigepos = %f, motorvalue = %f, refP2 = %f, huidigepos2 = %f, motorvalue2 = %f \r\n", refP, Huidigepositie, motorValue, refP2, Huidigepositie2, Huidigepositie2);
+    pc.printf("refP = %f, huidigepos = %f, motorvalue = %f, refP2 = %f, huidigepos2 = %f, motorvalue2 = %f \r\n", refP, Huidigepositie1, motorValue1, refP2, Huidigepositie2, Huidigepositie2);
+        
 }
+
+void direction()
+{ 
+    if (butsw2==0) {
+        if (A==true){// zodat het knopje 1 x wordt afgelezen
+        // reverses the direction
+        motor1_direction.write(direction1 = !direction1);
+        pc.printf("direction: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
+        A=false;
+        }
+    }
+    else{
+    A=true;
+    }
     
-void measure_signals()
+    if (butsw3==0){
+        if (B==true){
+        motor2_direction.write(direction2 = !direction2);
+        pc.printf("direction: %s\r\n\n", direction2 ? "clockwise" : "counter clockwise");
+        B=false;
+        }
+    }
+    else{
+    B=true;
+}
+}
+
+/*void measure_signals()
 {
     enc1_value = enc1.getPosition();
     enc2_value = enc2.getPosition();
-    enc1_value -= enc1_zero;
-    enc2_value -= enc2_zero;
+    //enc1_value -= enc1_zero;
+    //enc2_value -= enc2_zero;
     theta1 = (float)(enc1_value)/(float)(8400)*2*PI;
     theta2 = (float)(enc2_value)/(float)(8400)*2*PI;    
-  
     }  
-  
+*/  
 void state_machine()
 {
     //run current state
     switch (state) {
         case s_idle:
-            do_nothing();
+            rest();
             break;
-        case s_cali_enc:
-            cali_enc();
+        case s_calibratie_encoder:
+            calibratie_encoder();
             break;
         case s_demo:
-        MeasureAndControl();
+            MeasureAndControl();
+            direction();
         break;
         
 }
@@ -209,7 +241,7 @@
         failure_occurred = true;
     }
     but1_pressed = true;
-    pc.printf("Button 1 pressed \n\r");
+    pc.printf("Button 1 pressed \n\r"); 
 }
  
 void but2_interrupt()
@@ -220,10 +252,10 @@
     but2_pressed = true;
     pc.printf("Button 2 pressed \n\r");
 }
-
+    
 void main_loop()
 {
-    measure_signals();
+    //measure_signals();
     state_machine();
 }