met knopjes, voor Wubs, zit PID in dus restrictie.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of a_pid_kal_end_def by
main.cpp
- Committer:
- daniQQue
- Date:
- 2016-11-14
- Revision:
- 60:c165691c4e86
- Parent:
- 59:1725a3f02f37
File content as of revision 60:c165691c4e86:
//======================================================================================================================================================= //libraries #include "mbed.h" //mbed revision 113 #include "HIDScope.h" //Hidscope by Tom Lankhorst #include "BiQuad.h" //BiQuad by Tom Lankhorst #include "MODSERIAL.h" //Modserial #include "QEI.h" //QEI library for the encoders //======================================================================================================================================================= //Define objects //Tickers Ticker ticker_referenceangle; //ticker for the reference angle Ticker ticker_controllerm1; //ticker for the controller (PID) of motor 1 Ticker ticker_encoder; //ticker for encoderfunction motor 1 //Timer Timer timer; //Monitoring MODSERIAL pc(USBTX, USBRX); //pc connection DigitalOut red(LED_RED); //LED on K64F board, 1 is out; 0 is on DigitalOut green(LED_GREEN); //LED on K64f board, 1 is out; o is on //buttons DigitalIn button__right_biceps(D9); DigitalIn button__left_biceps(PTC12); InterruptIn button_switch(SW3); //motors DigitalOut richting_motor1(D7); //motor 1 connected to motor 1 at k64f board; for turningtable PwmOut pwm_motor1(D6); DigitalOut richting_motor2(D4); //motor 2 connected to motor 2 at k64f board; for linear actuator PwmOut pwm_motor2(D5); //encoders DigitalIn encoder1A(D13); DigitalIn encoder1B(D12); //controller BiQuad PID_controller; //======================================================================================================================================================= //define variables //on/off and switch signals int switch_signal = 0; //start of counter, switch made by even and odd numbers //motorvariables float speedmotor1=0.18; //speed of motor 1 is 0.18 pwm at start float speedmotor2=1.0; //speed of motor 2 is 1.0 pwm at start int cw=0; //clockwise direction int ccw=1; //counterclockwise direction //encoder int counts_encoder1; //variable to count the pulses given by the encoder, 1 indicates motor 1 float rev_counts_motor1; //calculated revolutions float rev_counts_motor1_rad; //calculated revolutions in rad! const float gearboxratio=131.25; //gearboxratio from encoder to motor const float rev_rond=64.0; //number of revolutions per rotation QEI Encoder1(D13,D12,NC,rev_rond,QEI::X4_ENCODING); //To set the encoder //reference volatile float d_ref = 0; //reference angle, starts off 0 const float w_ref = 1.5; //reference speed, constant volatile double t_start; //starttime of the timer volatile double w_var; //variable reference speed for making the reference signal const double Ts = 0.001; //time step for diverse tickers. It is comparable to a frequency of 1000Hz //controller const double Kp = 1.2614; //calculated value for the proportional action of the PID const double Ki = 0.4219; //calculated value for the integral action of the PID const double Kd = 0.8312; //calculated value for the derivative action of the PID const double N = 100; //specified value for the filter coefficient of the PID volatile double error1; //calculated error volatile double controlOutput; //output of the PID-controller bool start_motor = true; //bool to start the reference when the motor turns //======================================================================================================================================================= //======================================================================================================================================================= //voids //======================================================================================================================================================= //function teller void switch_function() { // The switch function. Makes it possible to switch between the motors. It simply adds one at switch_signal. if(button_switch==0){ switch_signal++; // To monitor what is happening: we will show the text in putty and change led color from red to green or vice versa. green=!green; red=!red; if (switch_signal%2==0){ pc.printf("If you push button 1 , the robot will go right \r\n"); pc.printf("If you push button 2, the robot will go left \r\n"); pc.printf("\r\n"); } else{ pc.printf("If you push button 1, the robot will go up \r\n"); pc.printf("If you push button 2, the robot will go down \r\n"); pc.printf("\r\n"); } } } //======================================================================================================================================================= //reference void makes the reference that the controllor should follow. There is only a controller for motor 1. void reference(){ if (start_motor == true){ //bool that is true when the motor starts turning timer.start(); //timer that starts counting in milliseconds } if (button__left_biceps==0 && switch_signal%2==0){ //the signal of the biceps is -1 and the switch is even, so motor 1 is being controlled t_start = timer.read_ms(); //read the current time passed from the timer start_motor = false; //it means that the motor is not running or has started up if (t_start < 1.0){ //the time passed is less than one second w_var = t_start*1.5; //the reference velocity is the time passed multiplied with the eventual constant velocity it should reach } else{ w_var = 1.5; //if the time passed is more than one second, the velocity is constant } d_ref = d_ref + w_var * Ts; //makes the reference angle } if (d_ref > 12){ //set the restrictions d_ref = 12; start_motor = true; //after the restriction is reached the motor (if turned the other way) will start up again so the bool has to be set to true } else{ d_ref = d_ref; //if there is no signal, the referance angle is constant } if (button__right_biceps==0 && switch_signal%2==0){ //the signal of the biceps is -1 and the switch is even, so motor 1 is being controlled t_start = timer.read_ms(); start_motor = false; if (t_start < 1.0){ w_var = t_start*1.5; } else { w_var = 1.5; } d_ref = d_ref - w_var * Ts; //the motor should turn the other way now so the reference becomes negative } if (d_ref < -12){ //negative restriction d_ref = -12; start_motor = true; } else{ d_ref = d_ref; } } //======================================================================================================================================================= //This void calculates the error and makes the control output. void m1_controller(){ error1 = d_ref-rev_counts_motor1_rad; //calculate the error = reference-output controlOutput = PID_controller.step(error1); //give the error as input to the controller } //======================================================================================================================================================= //This void calculated the number of rotations that the motor has done in rad. It is put in a void because with the ticker, this ensures that it is updated continuously. void encoders(){ counts_encoder1 = Encoder1.getPulses(); rev_counts_motor1 = (float)counts_encoder1/(gearboxratio*rev_rond); rev_counts_motor1_rad = rev_counts_motor1*6.28318530718; } //======================================================================================================================================================= //program //======================================================================================================================================================= int main(){ pc.baud(115200); //connect with pc with baudrate 115200 green=1; //led is off (1), at beginning red=0; //led is on (0), at beginning //attach tickers to functions ticker_referenceangle.attach(&reference, Ts); ticker_controllerm1.attach(&m1_controller, Ts); ticker_encoder.attach(&encoders, Ts); //PID controller PID_controller.PIDF(Kp,Ki,Kd,N,Ts); //Encoder //QEI Encoder1(D13,D12, NC, rev_rond,QEI::X4_ENCODING); //Show the user what the starting motor will be and what will happen pc.printf("We will start the demonstration\r\n"); pc.printf("\r\n\r\n\r\n"); if (switch_signal%2==0){ pc.printf("If you contract the biceps, the robot will go right \r\n"); pc.printf("If you contract the triceps, the robot will go left \r\n"); pc.printf("\r\n"); } else{ pc.printf("If you contract the biceps, the robot will go up \r\n"); pc.printf("If you contract the triceps, the robot will go down \r\n"); pc.printf("\r\n"); } //======================================================================================================================================================= //endless loop while (true) { //neverending loop button_switch.fall(&switch_function); if (button__left_biceps==0){ //left biceps contracted if (switch_signal%2==0){ //switch even speedmotor1=controlOutput; //output PID-controller is the speed for motor1 if (speedmotor1<0){ //if the output of the controller is negative, the direction is clockwise richting_motor1 = cw; //motor 1, right } else { //if the output is positive, the direction is counterclockwise richting_motor1 = ccw; //motor 1, left } pwm_motor1 = fabs(speedmotor1); //speed of motor 1, absolute because pwm cannot be negative } else{ //switch odd richting_motor2 = ccw; //motor 2, up pwm_motor2 = speedmotor2; //speed of motor 2 } } else if (button__right_biceps==0){ //right biceps contracted if (switch_signal%2==0){ //switch signal even speedmotor1=controlOutput; if (speedmotor1<0){ //the same as for the left biceps, the robot turns in the right direction because of the reference signal richting_motor1 = cw; //motor 1, right } else { richting_motor1 = ccw; //motor 1, left } pwm_motor1 = fabs(speedmotor1); //speed of motor 1 } else{ //switch signal odd richting_motor2 = cw; //motor 2, down pwm_motor2 = speedmotor2; //speed motor 2 } } else{ //no contraction of biceps, thus no motoraction. pwm_motor2=0; pwm_motor1=0; start_motor = true; //every time the motor is off, the bool is reset so that the reference void can start when the motor starts } } //while true closed } //int main closed //=======================================================================================================================================================