Biorobotics / Mbed 2 deprecated Controllerfunction

movemotor function. still to finish input variables and how to pass PWM and Direction pin and QEI

Dependencies:   QEI mbed

movemotor.cpp@0:7098cdb29180, 2015-10-19 (annotated)

Committer:
roosbulthuis
Date:
Mon Oct 19 13:43:58 2015 +0000
Revision:
0:7098cdb29180
function for motor control we still need values for inputs and we dont know how to refer to PWM and directionPin. And how can we declare QEI wheel differently for two motors?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
roosbulthuis 0:7098cdb29180 1 #include "QEI.h"
roosbulthuis 0:7098cdb29180 2 #include "mbed.h"
roosbulthuis 0:7098cdb29180 3
roosbulthuis 0:7098cdb29180 4 // Defining pulses per revolution (calculating pulses to rotations in degree.)
roosbulthuis 0:7098cdb29180 5 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)
roosbulthuis 0:7098cdb29180 6 const double M1_timestep = 0.01; // reason ticker works with 100 Hz. ASSUMING TIME STEP IS THE SAME FOR BOTH MOTORS
roosbulthuis 0:7098cdb29180 7
roosbulthuis 0:7098cdb29180 8 // define rotation direction and begin possition
roosbulthuis 0:7098cdb29180 9 const int cw = 1;
roosbulthuis 0:7098cdb29180 10 const int ccw = 0;
roosbulthuis 0:7098cdb29180 11
roosbulthuis 0:7098cdb29180 12 // define ticker
roosbulthuis 0:7098cdb29180 13 Ticker aansturen;
roosbulthuis 0:7098cdb29180 14
roosbulthuis 0:7098cdb29180 15 QEI wheel (PTC10, PTC11, NC, 624); // Pin for counting (analog in) // ask victor whether it can be in if-loop for the two different motors
roosbulthuis 0:7098cdb29180 16
roosbulthuis 0:7098cdb29180 17 // defining flags
roosbulthuis 0:7098cdb29180 18 volatile bool flag_motor = false;
roosbulthuis 0:7098cdb29180 19
roosbulthuis 0:7098cdb29180 20
roosbulthuis 0:7098cdb29180 21 // making function flags.
roosbulthuis 0:7098cdb29180 22 void Go_flag_motor()
roosbulthuis 0:7098cdb29180 23 {
roosbulthuis 0:7098cdb29180 24 flag_motor = true;
roosbulthuis 0:7098cdb29180 25 }
roosbulthuis 0:7098cdb29180 26
roosbulthuis 0:7098cdb29180 27
roosbulthuis 0:7098cdb29180 28 // defining reusable P controller
roosbulthuis 0:7098cdb29180 29 double PI(double error, const double Kp, const double Ki, double Ts, double &e_int)
roosbulthuis 0:7098cdb29180 30 {
roosbulthuis 0:7098cdb29180 31
roosbulthuis 0:7098cdb29180 32 e_int = e_int + Ts * error;
roosbulthuis 0:7098cdb29180 33
roosbulthuis 0:7098cdb29180 34 double PI_output = (Kp * error) + (Ki * e_int);
roosbulthuis 0:7098cdb29180 35 return PI_output;
roosbulthuis 0:7098cdb29180 36 }
roosbulthuis 0:7098cdb29180 37
roosbulthuis 0:7098cdb29180 38 // Measure the error and apply the output to the plant
roosbulthuis 0:7098cdb29180 39 void motor1_Controller(DigitalOut directionPin(), PwmOut PWM(), double position, double Setpoint, double Kp, double Ki) //How to pass Digitalout and PWM, by reference?
roosbulthuis 0:7098cdb29180 40 {
roosbulthuis 0:7098cdb29180 41 double reference = setpoint; // setpoint is in pulses
roosbulthuis 0:7098cdb29180 42 double position = wheel.getPulses();
roosbulthuis 0:7098cdb29180 43 double error_pulses = (reference - position); // calculate the error in pulses
roosbulthuis 0:7098cdb29180 44 double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn
roosbulthuis 0:7098cdb29180 45
roosbulthuis 0:7098cdb29180 46 double output = abs(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal ));
roosbulthuis 0:7098cdb29180 47
roosbulthuis 0:7098cdb29180 48 if(error_pulses > 0) {
roosbulthuis 0:7098cdb29180 49 directionPin.write(cw);
roosbulthuis 0:7098cdb29180 50 }
roosbulthuis 0:7098cdb29180 51 else if(error_pulses < 0) {
roosbulthuis 0:7098cdb29180 52 directionPin.write(ccw);
roosbulthuis 0:7098cdb29180 53 }
roosbulthuis 0:7098cdb29180 54 else{
roosbulthuis 0:7098cdb29180 55 output = 0;
roosbulthuis 0:7098cdb29180 56 }
roosbulthuis 0:7098cdb29180 57 PWM.write(output); // out of the if loop due to abs output
roosbulthuis 0:7098cdb29180 58 }
roosbulthuis 0:7098cdb29180 59
roosbulthuis 0:7098cdb29180 60
roosbulthuis 0:7098cdb29180 61
roosbulthuis 0:7098cdb29180 62 void move_motor(std::string state) //state can be left, right, or keypress
roosbulthuis 0:7098cdb29180 63 {
roosbulthuis 0:7098cdb29180 64
roosbulthuis 0:7098cdb29180 65 if (state == "left")
roosbulthuis 0:7098cdb29180 66 {
roosbulthuis 0:7098cdb29180 67 DigitalOut directionPin(D4); //direction
roosbulthuis 0:7098cdb29180 68 PwmOut PWM(D5); //speed
roosbulthuis 0:7098cdb29180 69 double position = wheel.getPulses(); //actual position
roosbulthuis 0:7098cdb29180 70 double Setpoint; //desired position
roosbulthuis 0:7098cdb29180 71 Kp; //controller gain
roosbulthuis 0:7098cdb29180 72 Ki; //set to zero
roosbulthuis 0:7098cdb29180 73 }
roosbulthuis 0:7098cdb29180 74
roosbulthuis 0:7098cdb29180 75 else if (state == "right")
roosbulthuis 0:7098cdb29180 76 {
roosbulthuis 0:7098cdb29180 77 DigitalOut directionPin(D4);
roosbulthuis 0:7098cdb29180 78 PwmOut PWM(D5);
roosbulthuis 0:7098cdb29180 79 double position = wheel.getPulses(); //actual position
roosbulthuis 0:7098cdb29180 80 double Setpoint; //desired position
roosbulthuis 0:7098cdb29180 81 double Kp; //controller gain, same as for left
roosbulthuis 0:7098cdb29180 82 double Ki; //set to zero
roosbulthuis 0:7098cdb29180 83 }
roosbulthuis 0:7098cdb29180 84
roosbulthuis 0:7098cdb29180 85 else if (state == "keypress") //different encoder than left and right
roosbulthuis 0:7098cdb29180 86 {
roosbulthuis 0:7098cdb29180 87 DigitalOut directionPin(D6); //direction
roosbulthuis 0:7098cdb29180 88 PwmOut PWM(D7); //speed
roosbulthuis 0:7098cdb29180 89 double position = wheel.getPulses(); //actual position, but different motor!
roosbulthuis 0:7098cdb29180 90 double Setpoint; //desired position, always the same actually
roosbulthuis 0:7098cdb29180 91 Kp; //controller gain, different from left and right
roosbulthuis 0:7098cdb29180 92 Ki; //set to zero
roosbulthuis 0:7098cdb29180 93 }
roosbulthuis 0:7098cdb29180 94
roosbulthuis 0:7098cdb29180 95
roosbulthuis 0:7098cdb29180 96
roosbulthuis 0:7098cdb29180 97 //call the flags and p-controller
roosbulthuis 0:7098cdb29180 98
roosbulthuis 0:7098cdb29180 99
roosbulthuis 0:7098cdb29180 100 aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning // attaching the flag to the ticker
roosbulthuis 0:7098cdb29180 101 while (1) //error is not between -0.05 and 0.05 or something error should be a maximum of 3 mm
roosbulthuis 0:7098cdb29180 102 {
roosbulthuis 0:7098cdb29180 103 // to make the motor move
roosbulthuis 0:7098cdb29180 104 if(flag_motor) {
roosbulthuis 0:7098cdb29180 105 flag_motor = false;
roosbulthuis 0:7098cdb29180 106 motor1_Controller(directionPin, PWM, position, Setpoint, Kp, Ki); //How to pass Digitalout and PWM, by reference?
roosbulthuis 0:7098cdb29180 107 }
roosbulthuis 0:7098cdb29180 108 }
roosbulthuis 0:7098cdb29180 109 }