Biorobotics
/
Controller
Controller futhers (PI)
main.cpp
- Committer:
- NickDGreg
- Date:
- 2015-10-27
- Revision:
- 11:e2b2ea8a3be8
- Parent:
- 10:3796b6a6c239
- Child:
- 12:614f1439e679
File content as of revision 11:e2b2ea8a3be8:
#include "mbed.h" #include "QEI.h" Serial pc(USBTX, USBRX); //QEI wheel_one (PTC10, PTC11, NC, 624); // Pin for counting (analog in) //QEI wheel_one (PTD3, PTD1, NC, 624); QEI wheel_two (PTD0, PTD2, NC, 624); // Define pin for motor control DigitalOut directionPin_one(D4); PwmOut PWM_one(D5); PwmOut PWM_two(D6); DigitalOut directionPin_two(D7); DigitalIn buttonccw(PTA4); DigitalIn buttoncw(PTC6); // define ticker Ticker aansturen; Ticker Printen; // define rotation direction and begin possition const int cw = 1; const int ccw = 0; double setpoint_one = 0; //setpoint is in pulses double setpoint_two = 0; //"" "" // saying buttons are not pressed const int Buttoncw_pressed = 0; const int Buttonccw_pressed = 0; // Controller gain proportional and intergrator const double motor1_Kp = 5; // more or les random number. const double motor1_Ki = 0; const double M1_timestep = 0.01; // reason ticker works with 100 Hz. double motor1_error_integraal = 0; // initial value of integral error // Defining pulses per revolution (calculating pulses to rotations in degree.) const double pulses_per_revolution = 4200 ;//8400 counts is aangegeven op de motor for x4. 10 - 30 counts oveshoot. for moter 1(tape)! Motor 2 almost the same(nice) /* double Rotation = -2; // rotation double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree. */ // defining flags volatile bool flag_motor = false; volatile bool flag_pcprint = false; // making function flags. void Go_flag_motor() { flag_motor = true; } void Go_flag_pcprint() { flag_pcprint = true; } // To make a new setpoint double making_setpoint(double direction, double setpoint){ if ( cw == direction){ setpoint = setpoint + (pulses_per_revolution/400000); } if ( ccw == direction){ setpoint = setpoint - (pulses_per_revolution/400000); } return(setpoint); } // Reusable P controller double PI(double error, const double Kp, const double Ki, double Ts, double &e_int) { e_int = e_int + Ts * error; double PI_output = (Kp * error) + (Ki * e_int); return PI_output; } // Next task, measure the error and apply the output to the plant void motor1_Controller(DigitalOut directionPin, PwmOut PWM, QEI &wheel, double setpoint) { double reference = setpoint; // setpoint is in pulses double position = wheel.getPulses(); double error_pulses = (reference - position); // calculate the error in pulses double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn double output = abs(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal )); if(error_pulses > 0) { directionPin.write(cw); } else if(error_pulses < 0) { directionPin.write(ccw); } else{ output = 0; } PWM.write(output); // out of the if loop due to abs output } void counts_showing() { //double kijken = wheel_one.getPulses()/pulses_per_revolution; //pc.printf("ref %.2f rounds%.2f \n",setpoint_one/pulses_per_revolution,kijken); double kijken = wheel_two.getPulses()/pulses_per_revolution; double pulses = wheel_two.getPulses(); pc.printf("ref %.2f rounds%.2f pulses %.2f \n",setpoint_two/pulses_per_revolution,kijken, pulses); } int main() { aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning Printen.attach(&Go_flag_pcprint,0.1f); // 10 Hz // printstatement here because printing cost to much time. the motor void wouldn't be completed while( 1 ) { // to make the motor move if(flag_motor) { flag_motor = false; //motor1_Controller(directionPin_one, PWM_one, wheel_one, setpoint_one); motor1_Controller(directionPin_two, PWM_two, wheel_two, setpoint_two); } if(flag_pcprint) { flag_pcprint = false; counts_showing(); } // Pussing buttons to get input s0ignal /* ` while(buttoncw.read() == Buttoncw_pressed and buttonccw.read() == Buttonccw_pressed) { setpoint_two = making_setpoint( */ if(buttoncw.read() == Buttoncw_pressed) { //setpoint_one = making_setpoint(cw, setpoint_one); setpoint_two = making_setpoint(ccw, setpoint_two); } if(buttonccw.read() == Buttonccw_pressed) { //setpoint_one = making_setpoint(ccw, setpoint_one); setpoint_two = making_setpoint(cw, setpoint_two); } } }