Biorobotics
/
Controller
Controller futhers (PI)
main.cpp
- Committer:
- yohoo15
- Date:
- 2015-10-08
- Revision:
- 0:2b481a8db14f
- Child:
- 1:71617831bc4e
File content as of revision 0:2b481a8db14f:
#include "mbed.h" #include "QEI.h" Serial pc(USBTX, USBRX); QEI wheel (PTC10, PTC11, NC, 624); // Pin for counting (analog in) // Define pin for motor control DigitalOut directionPin(D4); PwmOut PWM(D5); // define ticker Ticker aansturen; // define rotation direction const int cw = 1; const int ccw = 0; // Controller gain proportional const double motor1_Kp = 2.5; // calculating pulses to rotations in degree. const double Offset = 10341 ;//8400 counts is aangegeven op de motor ( is te weinig) 10511 schiet iets over const double resolution = Offset / 360; double Rotation = 4; // rotation in degree double movement = Rotation * 360 * resolution; // times 360 to make Rotations degree. // Reusable P controller double P(double error, double Kp) { double error_normalised_degree = error / resolution; // calculate how much degree the motor has to turn. double error_normalised_rotation = error_normalised_degree / 360; //calculate how much the rotaions the motor has to turn double P_output = Kp * error_normalised_rotation; return P_output; } // Next task, measure the error and apply the output to the plant void motor1_Controller() { double reference = movement; double position = wheel.getPulses(); double error = reference - position; double output = P( error, motor1_Kp ); if(error > 0) { directionPin.write(cw); PWM.write(output); } else { directionPin.write(ccw); PWM.write(output); } } int main() { aansturen.attach( &motor1_Controller, 0.01f ); // 100 Hz while( 1 ) { } }