Diff: main.cpp

Revision:

0:67f1f06bdf17

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 21 11:21:29 2016 +0000
@@ -0,0 +1,93 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+
+
+MODSERIAL pc(USBTX, USBRX);
+DigitalOut motor_horizontal_dir(D7);   //Moet nog aangepast worden aan de motoren.
+PwmOut motor_horizontal_pwm(D6);
+DigitalOut motor_vertical_dir(D10);   //Moet nog aangepast worden aan de motoren.
+PwmOut motor_vertical_pwm(D11);
+
+QEI Encoder_hor(D13,D12,NC,8400);     //For the encoder
+QEI Encoder_ver(*,*,NC,8400);
+int Pulse_state_hor;
+int Previous_pulse_state_hor;
+double Pulse_destination_hor = 0;
+int Pulse_state_ver;
+int Previous_pulse_state_ver;
+double Pulse_destination_ver = 0;
+
+DigitalIn Button1(SW2);         //Buttons voor testen in plaats van elektrodes.
+DigitalIn Button2(SW3);
+
+float Elektrode_1;              //Niet nodig voor de test, input vanuit arm.
+float Elektrode_2;
+bool Elektrode_3;
+
+float Wait_time = 0.02f;
+float Sensitivity = 10;
+bool Control_active;
+int Arm_state = 1;
+
+float P = 1;                  //Goede waardes vinden.
+float I = 0.01;
+float D = 0.05;
+double Integrated_distance = 0;
+double PID_value = 0;
+
+Ticker Motor_control;      //Testen of er niks afgekapt wordt en voor een goede tijd.
+
+void Control_activator{
+    Control_active = true;
+}
+
+double Pid_controller(int Pulse_state,int Previous_pulse_state,double Pulse_destination) {
+    Integrated_distance = Integrated_distance + (Pulse_destination - Pulse_state) * Wait_time;
+    PID_value = (P * (Pulse_destination - Pulse_state) + I * Integrated_distance + D * (Previous_pulse_state - Pulse_state)/Wait_time)/500;
+    return PID_value;
+}
+
+void setmotor1(double val){
+    motor_horizontal_dir.write(val>=0?1:0);
+    motor_horizontal_pwm.write(fabs(val)>1;1.0:fabs(val));
+}
+
+void setmotor2(double val){
+    motor_vertical_dir.write(val>=0?1:0);
+    motor_vertical_pwm.write(fabs(val)>1;1.0:fabs(val));
+}
+
+int main()
+{
+    pc.baud(115200);
+    Motor_control.attach(&Control_activator,Wait_time);
+    while (true) {
+        Previous_pulse_state_hor = Pulse_state_hor;
+        Pulse_state_hor =  Encoder_hor.getPulses();
+        Previous_pulse_state_ver = Pulse_state_ver;
+        Pulse_state_ver =  Encoder_ver.getPulses();
+        if Control_active(){
+            switch Arm_state{
+                case "1"
+                    
+                    break
+                case "2"
+                    
+                    break
+                case "3"
+                    
+                    break
+                case "4"
+                    
+                    break
+            double PID_value_ver = PID_controller(Pulse_state_ver,Previous_pulse_state_ver,Pulse_destination_ver);
+            setmotor2(PID_value_hor);
+            double PID_value_hor = PID_controller(Pulse_state_hor,Previous_pulse_state_hor,Pulse_destination_hor);
+            setmotor1(PID_value_hor);
+            Control_active = false;
+            }
+        }
+        wait(0.01f);
+    }
+}
\ No newline at end of file