main.cpp

00001 #include "mbed.h"
00002 #include "MODSERIAL.h"
00003 #include "QEI.h"
00004 
00005 
00006 MODSERIAL pc(USBTX, USBRX);
00007 DigitalOut motor_horizontal_dir(D7);   //Moet nog aangepast worden aan de motoren.
00008 PwmOut motor_horizontal_pwm(D6);
00009 DigitalOut motor_vertical_dir(D10);   //Moet nog aangepast worden aan de motoren.
00010 PwmOut motor_vertical_pwm(D11);
00011 
00012 QEI Encoder_hor(D13,D12,NC,8400);     //For the encoder
00013 QEI Encoder_ver(*,*,NC,8400);
00014 int Pulse_state_hor;
00015 int Previous_pulse_state_hor;
00016 double Pulse_destination_hor = 0;
00017 int Pulse_state_ver;
00018 int Previous_pulse_state_ver;
00019 double Pulse_destination_ver = 0;
00020 
00021 DigitalIn Button1(SW2);         //Buttons voor testen in plaats van elektrodes.
00022 DigitalIn Button2(SW3);
00023 
00024 float Elektrode_1;              //Niet nodig voor de test, input vanuit arm.
00025 float Elektrode_2;
00026 bool Elektrode_3;
00027 
00028 float Wait_time = 0.02f;
00029 float Sensitivity = 10;
00030 bool Control_active;
00031 int Arm_state = 1;
00032 
00033 float P = 1;                  //Goede waardes vinden.
00034 float I = 0.01;
00035 float D = 0.05;
00036 double Integrated_distance = 0;
00037 double PID_value = 0;
00038 
00039 Ticker Motor_control;      //Testen of er niks afgekapt wordt en voor een goede tijd.
00040 
00041 void Control_activator{
00042     Control_active = true;
00043 }
00044 
00045 double Pid_controller(int Pulse_state,int Previous_pulse_state,double Pulse_destination) {
00046     Integrated_distance = Integrated_distance + (Pulse_destination - Pulse_state) * Wait_time;
00047     PID_value = (P * (Pulse_destination - Pulse_state) + I * Integrated_distance + D * (Previous_pulse_state - Pulse_state)/Wait_time)/500;
00048     return PID_value;
00049 }
00050 
00051 void setmotor1(double val){
00052     motor_horizontal_dir.write(val>=0?1:0);
00053     motor_horizontal_pwm.write(fabs(val)>1;1.0:fabs(val));
00054 }
00055 
00056 void setmotor2(double val){
00057     motor_vertical_dir.write(val>=0?1:0);
00058     motor_vertical_pwm.write(fabs(val)>1;1.0:fabs(val));
00059 }
00060 
00061 int main()
00062 {
00063     pc.baud(115200);
00064     Motor_control.attach(&Control_activator,Wait_time);
00065     while (true) {
00066         Previous_pulse_state_hor = Pulse_state_hor;
00067         Pulse_state_hor =  Encoder_hor.getPulses();
00068         Previous_pulse_state_ver = Pulse_state_ver;
00069         Pulse_state_ver =  Encoder_ver.getPulses();
00070         if Control_active(){
00071             switch Arm_state{
00072                 case "1"
00073                     
00074                     break
00075                 case "2"
00076                     
00077                     break
00078                 case "3"
00079                     
00080                     break
00081                 case "4"
00082                     
00083                     break
00084             double PID_value_ver = PID_controller(Pulse_state_ver,Previous_pulse_state_ver,Pulse_destination_ver);
00085             setmotor2(PID_value_hor);
00086             double PID_value_hor = PID_controller(Pulse_state_hor,Previous_pulse_state_hor,Pulse_destination_hor);
00087             setmotor1(PID_value_hor);
00088             Control_active = false;
00089             }
00090         }
00091         wait(0.01f);
00092     }
00093 }