Diff: main.cpp

Revision:

20:ac1b4ffa3323

Parent:

19:5ac8b7af77a3

Child:

21:394a7a1deb73

--- a/main.cpp	Fri Oct 04 09:37:33 2019 +0000
+++ b/main.cpp	Fri Oct 04 13:28:00 2019 +0000
@@ -1,5 +1,6 @@
 #include "QEI.h"
 #include "mbed.h"
+#include "BiQuad.h"
 #include "FastPWM.h"
 #include "HIDScope.h"
 #include "MODSERIAL.h"
@@ -11,169 +12,114 @@
 // BUT1 --> D1
 // BUT2 --> D0
 
-MODSERIAL pc(USBTX, USBRX);
-DigitalOut ledr(LED_RED);
-DigitalOut ledg(LED_GREEN);
-DigitalOut ledb(LED_BLUE);
-Ticker ReduceEmission;
-Timer R;
-Timer G;
-Timer B;
+MODSERIAL   pc(USBTX, USBRX);
+Ticker      ControlMotor1;
 
-FastPWM lichtje(D3);
-AnalogIn   ain(A0);
-DigitalOut PWM_motor_1(D6);
-DigitalOut direction_motor_1(D7);
+AnalogIn    theta_ref(A0);
+DigitalOut  PWM_motor_1(D6);
+DigitalOut  direction_motor_1(D7);
 
 HIDScope scope(2);
 QEI encoder(D12,D13,NC,64,QEI::X4_ENCODING);
 Ticker RW_scope;
 
 // variables
-
-volatile char command = 'r';
-enum states {red, green, blue};
-states CurrentState = red;
-bool StateChanged = true;
-
+float duty_cycle_motor_1;
 volatile int on_time_ms; // The time the LED should be on, in microseconds
 volatile int off_time_ms;
 
-int degrees;
-volatile double x;
-volatile double x_prev=0;
-volatile double y;
+float theta = 0;
+volatile double x_0;
+volatile double x_prev_0 = 0;
+volatile double y_0;
+volatile double x_1;
+volatile double x_prev_1 = 0;
+volatile double y_1; 
+
+float omega;
+float torque;
+float torque_p;
+float torque_i;
+float torque_d;
+float velocity;
+float theta_error;
+
+float Kp = 5;
+float Ki = 3;
+float Kd = 3;
+float Ts = 1;
 
 // functions
-
-void TurnLedRed(){
-    ledr = 0;
-    ledg = 1;
-    ledb = 1;}
+float CalculateError(){
+    //theta_error = (360*theta_ref.read())-theta;
+    theta_error = 180;
+    return theta_error;}
 
-void TurnLedGreen(){
-    ledr = 1;
-    ledg = 0;
-    ledb = 1;}
+float Controller(float theta_error){
+    //static float error_integral = 0;
+    //static float error_prev = 0;
+    //static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    
+    // Proportional part:
+    float torque_p = Kp * theta_error;
+    
+    // Integral part:
+    //error_integral = error_integral + theta_error * Ts;
+    //float torque_i = Ki * error_integral;
+    
+    // Derivative part:
+    //float error_derivative = (theta_error - error_prev)/Ts;
+    //float filtered_error_derivative = LowPassFilter.step(error_derivative);
+    //float torque_d = Kd * filtered_error_derivative;
+    //float torque_d = Kd*error_derivative;
+    //error_prev = theta_error;
+    
+    // Sum all parts and return it
+    //float torque = torque_p + torque_i + torque_d;
+    return torque_p;}
 
-void TurnLedBlue(){
-    ledr = 1;
-    ledg = 1;
-    ledb = 0;}
+void CalculateDirectionMotor1() {
+    if (theta_error >= 0) {
+        direction_motor_1 = 1;}
+    else {
+        direction_motor_1 = 0;}}
+
+//float CalculateVelocityMotor1() {
+//    return theta_error/Ts;}
 
 void PulseWidthModulation() // Bepaalt de duty cycle gebaseerd op de potentiometer en zet de motor dan op die snelheid mbh PWM
-{   on_time_ms = (int) (ain.read()*(1/1e2)*1e3);
-    off_time_ms = (int) ((1-ain.read())*(1/1e2)*1e3);
+{   on_time_ms = (int) (duty_cycle_motor_1*(1/1e2)*1e3);    // Deze variable maken
+    off_time_ms = (int) ((1-duty_cycle_motor_1)*(1/1e2)*1e3);
     PWM_motor_1 = 1;
     wait_ms(on_time_ms);
     PWM_motor_1 = 0;
     wait_ms(off_time_ms);}
-
-void EnergySaving() // Functie voor de "ReduceEmissions" ticker, zet de motor uit en LED op rood
-{    command = 'r';}
-
-void CheckCommandFromTerminal(void) // Functie voor de in de main loop
-{    command = pc.getcNb();}
-
-void WhileRed()
-{    if (command == 'g') {
-        R.stop();
-        pc.printf("The LED has been red for %f seconds!\n\r", R.read());
-        CurrentState= green;
-        StateChanged= true;    }
-    if (command == 'b') {
-        R.stop();
-        pc.printf("The LED has been red for %f seconds!\n\r", R.read());
-        CurrentState= blue;
-        StateChanged= true;    }}
-
-void WhileGreen()
-{   PulseWidthModulation();
-    if (command == 'r') {
-        G.stop();
-        pc.printf("The LED has been green for %f seconds!\n\r", G.read());
-        CurrentState= red;
-        StateChanged= true;    }
-    if (command == 'b') {
-        G.stop();
-        pc.printf("The LED has been green for %f seconds!\n\r", G.read());
-        CurrentState= blue;
-        StateChanged= true;    }}
-
-void WhileBlue()
-{   PulseWidthModulation();
-    if (command == 'r') {
-        B.stop();
-        pc.printf("The LED has been blue for %f seconds!\n\r", B.read());
-        CurrentState= red;
-        StateChanged= true;    }
-    if (command == 'g') {
-        B.stop();
-        pc.printf("The LED has been blue for %f seconds!\n\r", B.read());
-        CurrentState= green;
-        StateChanged= true;    }}
+    
+void MotorDirection(){
+    }
 
 void ReadEncoderAndWriteScope(){
-    degrees = ((360/64)*encoder.getPulses())/131.25; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360.
-    x = degrees;
-    scope.set(0,x);
-    y = (x_prev + x)/2.0;
-    scope.set(1,y);
-    x_prev=x;
+    theta = ((360/64)*encoder.getPulses())/131.25; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360.
+    x_0 = theta;
+    y_0 = (x_prev_0 + x_0)/2.0;
+    scope.set(0,y_0);
+    x_prev_0 = x_0;
+    y_1 = (x_0 - x_prev_0)/Ts;
+    scope.set(1,y_1);
     scope.send();}
 
-void StateMachine(void)
-{
-    switch(CurrentState) {
-        case red:
-            if  (StateChanged) {
-                pc.printf("Initiating turning LED red\n\r");
-                StateChanged= false;
-                TurnLedRed();
-                R.start();
-                PWM_motor_1 = 0;
-                pc.printf("LED is now red!\n\r");
-            }
-            WhileRed();
-            break;
-        case green:
-            if  (StateChanged) {
-                pc.printf("Initiating turning LED green\n\r");
-                StateChanged= false;
-                TurnLedGreen();
-                G.start();
-                direction_motor_1 = 0;
-                pc.printf("LED is now green!\n\r");
-            }
-            WhileGreen();
-            break;
-        case blue:
-            if  (StateChanged) {
-                pc.printf("Initiating turning LED blue\n\r");
-                StateChanged= false;
-                TurnLedBlue();
-                B.start();
-                direction_motor_1 = 255;
-                pc.printf("LED is now blue!\n\r");
-            }
-            WhileBlue();
-            break;
-        default:
-            pc.printf("Unknown or unimplemented state reached!\n\r");
-    }
-}
-
+//void doeietsnuttigs(){
+  //  Controller(CalculateError());}
+    
 // main
-
 int main()
 {
     pc.baud(115200);
     pc.printf("Hello World!\n\r");
-    RW_scope.attach(&ReadEncoderAndWriteScope, 0.1);
-    ReduceEmission.attach(EnergySaving,20);
-    while(true) {
-        CheckCommandFromTerminal();
-        StateMachine();
-        lichtje.write(ain.read());  // duty cycle
+    //ControlMotor1.attach(&ReadEncoderAndWriteScope, Ts);
+    //ControlMotor1.attach(&doeietsnuttigs, Ts);
+    while(1) {
+        pc.printf("%f \n\r",Controller(CalculateError()));
+        wait(1);
     }
 }
\ No newline at end of file