Diff: main.cpp

Revision:

21:394a7a1deb73

Parent:

20:ac1b4ffa3323

Child:

22:6cc93216b323

--- a/main.cpp	Fri Oct 04 13:28:00 2019 +0000
+++ b/main.cpp	Mon Oct 14 08:50:26 2019 +0000
@@ -5,121 +5,248 @@
 #include "HIDScope.h"
 #include "MODSERIAL.h"
 
-// ENC1A --> D13
-// ENC1B --> D12
-// POT1 --> A0
-// LED1 --> D3
-// BUT1 --> D1
-// BUT2 --> D0
-
-MODSERIAL   pc(USBTX, USBRX);
-Ticker      ControlMotor1;
-
-AnalogIn    theta_ref(A0);
-DigitalOut  PWM_motor_1(D6);
+// Pins
+MODSERIAL pc(USBTX, USBRX);
+InterruptIn stepresponse(D0);
+FastPWM     PWM_motor_1(D6);
 DigitalOut  direction_motor_1(D7);
-
-HIDScope scope(2);
-QEI encoder(D12,D13,NC,64,QEI::X4_ENCODING);
-Ticker RW_scope;
+InterruptIn button_1(SW2);
+InterruptIn button_2(SW3);
 
 // variables
-float duty_cycle_motor_1;
-volatile int on_time_ms; // The time the LED should be on, in microseconds
-volatile int off_time_ms;
-
-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; 
+Ticker          TickerStateMachine;
+Ticker          ControlMotor1;
+Ticker          RW_scope;
+Timer           sinus_time;
+enum    states  {start, motor_calibration, demo_mode, emg_calibration, vertical_movement, horizontal_movement};
+states  CurrentState = start;
+bool    StateChanged = true;
+bool    demo    = false;
+bool    emg     = false;
+bool    next    = false;
+HIDScope scope(3);
+QEI encoder(D12,D13,NC,8400,QEI::X4_ENCODING);
+volatile double theta;
+float   theta_error;
+volatile float theta_reference;
+float K     = 1;
+float ti    = 0.1;
+float td    = 10;
+float Kp    = K*(1+td/ti);
+float Ki    = K/ti;
+float Kd    = K*td;
+float Ts    = 0.001;
 
-float omega;
-float torque;
-float torque_p;
-float torque_i;
-float torque_d;
-float velocity;
-float theta_error;
+// functies
+float CalculateError()
+{
+    theta_error = theta_reference-theta;
+    return theta_error;
+}
 
-float Kp = 5;
-float Ki = 3;
-float Kd = 3;
-float Ts = 1;
-
-// functions
-float CalculateError(){
-    //theta_error = (360*theta_ref.read())-theta;
-    theta_error = 180;
-    return theta_error;}
-
-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);
+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;
+    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;
+    float error_derivative = (theta_error - error_prev)/Ts;
+    float filtered_error_derivative = LowPassFilter.step(error_derivative);
+    float torque_d = Kd * filtered_error_derivative;
+    error_prev = theta_error;
     
     // Sum all parts and return it
-    //float torque = torque_p + torque_i + torque_d;
-    return torque_p;}
+    float torque = torque_p + torque_i + torque_d;
+    return torque;
+}
 
-void CalculateDirectionMotor1() {
-    if (theta_error >= 0) {
-        direction_motor_1 = 1;}
-    else {
-        direction_motor_1 = 0;}}
+void CalculateDirectionMotor1() 
+{
+    direction_motor_1 = Controller(CalculateError()) <= 0.0f ;
+}
 
-//float CalculateVelocityMotor1() {
-//    return theta_error/Ts;}
+void WriteScope()
+{
+    scope.set(0,theta);
+    //scope.set(1,Controller(CalculateError()));
+    scope.set(1,CalculateError());
+    scope.set(2,theta_reference);
+    scope.send();
+}
 
-void PulseWidthModulation() // Bepaalt de duty cycle gebaseerd op de potentiometer en zet de motor dan op die snelheid mbh PWM
-{   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 MotorDirection(){
+void ReadEncoder(){
+    theta = ((360.0f/64.0f)*(float)encoder.getPulses())/131.25f; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360.
     }
 
-void ReadEncoderAndWriteScope(){
-    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 Motor_1() {
+    ReadEncoder();
+    theta_reference = 360*sin(0.1f*sinus_time.read()*2*3.14f);
+    CalculateDirectionMotor1();
+    PWM_motor_1.write(fabs(Controller(CalculateError())/360.0f));
+}
+
+void go_next_1()
+{
+    demo = !demo;
+}
+
+void go_next_2()
+{
+    emg = !emg;
+    next = emg;
+}
+
+void while_start()
+{
+    // Do startup stuff
+    CurrentState = motor_calibration;
+    StateChanged = true;
+}
+
+void while_motor_calibration()
+{
+    // Do motor calibration stuff
+    if (demo)   // bool aanmaken voor demo (switch oid aanmaken)
+    {
+        CurrentState = demo_mode;
+        StateChanged = true;
+    }
+    if (emg)    // bool aanmaken voor EMG (switch oid aanmaken)
+    {
+        CurrentState = emg_calibration;
+        StateChanged = true;
+    }
+}
+
+void while_demo_mode()
+{
+    // Do demo mode stuff
+    if (!demo)  // bool aanmaken voor demo (switch oid)
+    {
+        emg = true;
+    }
+    if (emg)    // bool aanmaken voor EMG (switch oid aanmaken)
+    {
+        CurrentState = emg_calibration;
+        StateChanged = true;
+    }
+}
+
+void while_emg_calibration()
+{
+    // Do emg calibration stuff
+    if (!emg)   // bool aanmaken voor EMG (switch)
+    {
+        CurrentState = vertical_movement;
+        StateChanged = true;
+    }
+}
+
+void while_vertical_movement()
+{
+    // Do vertical movement stuff
+    if (next)  // EMG gebaseerde threshold aanmaken
+    {
+        CurrentState = horizontal_movement;
+        StateChanged = true;
+    }
+}
 
-//void doeietsnuttigs(){
-  //  Controller(CalculateError());}
-    
+void while_horizontal_movement()
+{
+    // Do horizontal movement stuff
+    if (!next) // EMG gebaseerde threshold aanmaken
+    {
+        CurrentState = vertical_movement;
+        StateChanged = true;
+    }
+}
+
+void StateTransition()
+{
+    if (StateChanged)
+    {
+        if (CurrentState == start)
+        {
+            pc.printf("Initiating start.\n\r");
+        }
+        if (CurrentState == motor_calibration)
+        {
+            pc.printf("Initiating motor_calibration.\n\r");
+        }
+        if (CurrentState == demo_mode)
+        {
+            pc.printf("Initiating demo_mode.\n\r");
+        }
+        if (CurrentState == emg_calibration)
+        {
+            pc.printf("Initiating emg_calibration.\n\r");
+        }
+        if (CurrentState == vertical_movement)
+        {
+            pc.printf("Initiating vertical_movement.\n\r");
+        }
+        if (CurrentState == horizontal_movement)
+        {
+            pc.printf("Initiating horizontal_movement.\n\r");
+        }
+        StateChanged = false;
+    }
+}
+
+void StateMachine()
+{
+    switch(CurrentState)
+    {
+        case start:
+            StateTransition();
+            while_start();
+            break;
+        case motor_calibration:
+            StateTransition();
+            while_motor_calibration();
+            break;
+        case demo_mode:
+            StateTransition();
+            while_demo_mode();
+            break;
+        case emg_calibration:
+            StateTransition();
+            while_emg_calibration();
+            break;
+        case vertical_movement:
+            StateTransition();
+            while_vertical_movement();
+            break;
+        case horizontal_movement:
+            StateTransition();
+            while_horizontal_movement();
+            break;
+        default:
+            pc.printf("Unknown or unimplemented state reached!\n\r");
+    }
+}
+
 // main
-int main()
-{
+int main(){
     pc.baud(115200);
     pc.printf("Hello World!\n\r");
-    //ControlMotor1.attach(&ReadEncoderAndWriteScope, Ts);
-    //ControlMotor1.attach(&doeietsnuttigs, Ts);
-    while(1) {
-        pc.printf("%f \n\r",Controller(CalculateError()));
-        wait(1);
-    }
+    button_1.fall(go_next_1);
+    button_2.fall(go_next_2);
+    sinus_time.start();
+    PWM_motor_1.period_ms(10);
+    ControlMotor1.attach(&Motor_1, Ts);
+    RW_scope.attach(&WriteScope, 0.1);
+    //TickerStateMachine.attach(StateMachine,1.00f);
+    while(true) {
+        StateMachine();
+    }
 }
\ No newline at end of file