Biorobotics
/
Controller
Controller futhers (PI)
Diff: main.cpp
- Revision:
- 0:2b481a8db14f
- Child:
- 1:71617831bc4e
diff -r 000000000000 -r 2b481a8db14f main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Oct 08 11:35:49 2015 +0000 @@ -0,0 +1,68 @@ +#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 ) { } +} + + +