Diff: main.cpp

Revision:

2:5093b66c9b26

Parent:

1:b862262a9d14

Child:

3:16df793da2be

--- a/main.cpp	Wed Sep 04 15:30:13 2019 +0000
+++ b/main.cpp	Tue Oct 29 15:48:51 2019 +0000
@@ -1,23 +1,243 @@
+#include <math.h>
 #include "mbed.h"
-//#include "HIDScope.h"
-//#include "QEI.h"
 #include "MODSERIAL.h"
-//#include "BiQuad.h"
-//#include "FastPWM.h"
+#include "QEI.h"
+#include "FastPWM.h"
+#include "encoder.h"
+
+const double PI = 3.1415926535897;  
+
+InterruptIn but1(D2);
+InterruptIn but2(D3);
+AnalogIn potMeter1(A2);
+AnalogIn potMeter2(A1);
+
+DigitalOut motor1_direction(D4);                                                //value 0=CCW, 1=CW
+FastPWM motor1_pwm(D5);                                                          //Biorobotics Motor 1 PWM controle van de snelheid
+DigitalOut motor2_direction(D7);                                                //value 0=CCW, 1=CW
+FastPWM motor2_pwm(D6);
+
+Ticker loop_ticker; 
+// SERIAL COMMUNICATION WITH PC.................................................
+Serial pc(USBTX, USBRX);
+
+enum States {s_idle, s_cali_enc, s_demo};
+States state;
+
+// ENCODER .....................................................................
+Encoder enc1 (D13, D12, true); //encoder 1 gebruiken
+Encoder enc2 (D9,  D8,  true); //encoder 2 gebruiken 
+        
+float dt = 0.001;
+double e_int = 0;
+double e_int2 = 0;
+float theta1;
+float theta2;
+int enc1_zero = 0;//the zero position of the encoders, to be determined from the
+int enc2_zero = 0;//encoder calibration
+int enc1_value;
+int enc2_value;
+bool state_changed = false; 
+volatile bool but1_pressed = false;
+volatile bool but2_pressed = false;
+volatile bool failure_occurred = false;
+
+void do_nothing()
+    //Idle state. Used in the beginning, before the calibration states.  
+{
+    if (but1_pressed) {
+        state_changed = true;
+        state = s_cali_enc;
+        but1_pressed = false;
+    }
+}
 
-DigitalOut led(LED_RED);
+void cali_enc()
+{
+    if (state_changed) {
+        pc.printf("Started encoder calibration\r\n");
+        state_changed = false;
+    }
+    if (but1_pressed) {
+        pc.printf("Encoder has been calibrated \r\n");
+        enc1_zero = enc1_value;
+        enc2_zero = enc2_value;
+        but1_pressed = false;
+        state_changed = true;
+        state = s_demo;
+        pc.printf("enc01: %i\r\n", enc1_zero);
+        pc.printf("enc1value: %i\r\n", enc1_value);
+        pc.printf("enc02: %i\r\n",enc2_zero);
+        pc.printf("enc2value: %i\r\n", enc2_value);
+        pc.printf("hoek 1: %f\r\n",theta1);
+        pc.printf("hoek 2: %f\r\n",theta2);
+        
+ 
+    }
+}
+    
+double GetReferencePosition() 
+{
+    double Potmeterwaarde = potMeter1.read(); //naam moet universeel worden
+    int maxwaarde = 200;                   // = 64x64
+    double refP = Potmeterwaarde*maxwaarde;
+    return refP;                            // value between 0 and 4096 
+}
+ 
+double GetReferencePosition2() 
+{
+    double potmeterwaarde2 = potMeter2.read();
+    int maxwaarde2 = 200;                   // = 64x64
+    double refP2 = potmeterwaarde2*maxwaarde2;
+    return refP2;                            // value between 0 and 4096 
+}
+   
+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 ki = 0.0001;                           // kind of scaled.
+    e_int = e_int+dt*error;
+    double Integrator = ki*e_int;
+    
+    
+    double motorValue = Proportional + Integrator ;
+    return motorValue;
+}
+ 
+double FeedBackControl2(double error2, double &e_int2)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
+{
+    double kp2 = 0.002;                             // kind of scaled.
+    double Proportional2= kp2*error2;
+    
+    double ki2 = 0.00005;                           // kind of scaled.
+    e_int2 = e_int2+dt*error2;
+    double Integrator2 = ki2*e_int2;
+    
+    
+    double motorValue2 = Proportional2 + Integrator2 ;
+    return motorValue2;
+    
+}
+ 
+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 MeasureAndControl(void)
+{
+    //SetpointRobot(); 
+    // 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);
+    // 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);
+    SetMotor2(motorValue2);
+    pc.printf("refP = %f, huidigepos = %f, motorvalue = %f, refP2 = %f, huidigepos2 = %f, motorvalue2 = %f \r\n", refP, Huidigepositie, motorValue, refP2, Huidigepositie2, Huidigepositie2);
+}
+    
+void measure_signals()
+{
+    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;    
+  
+    }  
+  
+void state_machine()
+{
+    //run current state
+    switch (state) {
+        case s_idle:
+            do_nothing();
+            break;
+        case s_cali_enc:
+            cali_enc();
+            break;
+        case s_demo:
+        MeasureAndControl();
+        break;
+        
+}
+}
 
-MODSERIAL pc(USBTX, USBRX);
+void but1_interrupt()
+{
+    if(but2.read()) {//both buttons are pressed
+        failure_occurred = true;
+    }
+    but1_pressed = true;
+    pc.printf("Button 1 pressed \n\r");
+}
+ 
+void but2_interrupt()
+{
+    if(but1.read()) {//both buttons are pressed
+        failure_occurred = true;
+    }
+    but2_pressed = true;
+    pc.printf("Button 2 pressed \n\r");
+}
+
+void main_loop()
+{
+    measure_signals();
+    state_machine();
+}
 
 int main()
 {
     pc.baud(115200);
-    pc.printf("\r\nStarting...\r\n\r\n");
+    pc.printf("Executing main()... \r\n");
+    state = s_idle;
+    
     
-    while (true) {
-        
-        led = !led;
-        
-        wait_ms(500);
-    }
-}
+    but1.fall(&but1_interrupt);
+    but2.fall(&but2_interrupt);
+    loop_ticker.attach(&main_loop, dt);
+    pc.printf("main_loop is running\n\r");
+   
+  }  
+