Basis voor de pid controller van de arm.
Dependencies: MODSERIAL QEI mbed
main.cpp
- Committer:
- dirknefkens
- Date:
- 2016-10-21
- Revision:
- 0:67f1f06bdf17
File content as of revision 0:67f1f06bdf17:
#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); } }