Biorobotics / Mbed 2 deprecated Controller

Controller futhers (PI)

Dependencies:   QEI mbed

main.cpp@11:e2b2ea8a3be8, 2015-10-27 (annotated)

Committer:
NickDGreg
Date:
Tue Oct 27 14:46:32 2015 +0000
Revision:
11:e2b2ea8a3be8
Parent:
10:3796b6a6c239
Child:
12:614f1439e679
working for both motors, but need to hard code which motor is being controled. Next step is to treat left and right buttons as biceps and control the system that way.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yohoo15 0:2b481a8db14f 1 #include "mbed.h"
yohoo15 0:2b481a8db14f 2 #include "QEI.h"
yohoo15 0:2b481a8db14f 3
yohoo15 0:2b481a8db14f 4 Serial pc(USBTX, USBRX);
NickDGreg 10:3796b6a6c239 5 //QEI wheel_one (PTC10, PTC11, NC, 624); // Pin for counting (analog in)
NickDGreg 10:3796b6a6c239 6 //QEI wheel_one (PTD3, PTD1, NC, 624);
NickDGreg 10:3796b6a6c239 7 QEI wheel_two (PTD0, PTD2, NC, 624);
yohoo15 0:2b481a8db14f 8
yohoo15 0:2b481a8db14f 9 // Define pin for motor control
NickDGreg 9:ea9d35838861 10 DigitalOut directionPin_one(D4);
NickDGreg 9:ea9d35838861 11 PwmOut PWM_one(D5);
NickDGreg 9:ea9d35838861 12
NickDGreg 10:3796b6a6c239 13 PwmOut PWM_two(D6);
NickDGreg 10:3796b6a6c239 14 DigitalOut directionPin_two(D7);
NickDGreg 10:3796b6a6c239 15
yohoo15 7:6006a473ea0b 16 DigitalIn buttonccw(PTA4);
yohoo15 7:6006a473ea0b 17 DigitalIn buttoncw(PTC6);
yohoo15 0:2b481a8db14f 18
yohoo15 0:2b481a8db14f 19 // define ticker
yohoo15 0:2b481a8db14f 20 Ticker aansturen;
yohoo15 1:71617831bc4e 21 Ticker Printen;
yohoo15 0:2b481a8db14f 22
yohoo15 7:6006a473ea0b 23 // define rotation direction and begin possition
yohoo15 0:2b481a8db14f 24 const int cw = 1;
yohoo15 0:2b481a8db14f 25 const int ccw = 0;
NickDGreg 10:3796b6a6c239 26 double setpoint_one = 0; //setpoint is in pulses
NickDGreg 10:3796b6a6c239 27 double setpoint_two = 0; //"" ""
yohoo15 7:6006a473ea0b 28
yohoo15 7:6006a473ea0b 29 // saying buttons are not pressed
yohoo15 7:6006a473ea0b 30 const int Buttoncw_pressed = 0;
yohoo15 7:6006a473ea0b 31 const int Buttonccw_pressed = 0;
yohoo15 7:6006a473ea0b 32
yohoo15 0:2b481a8db14f 33
yohoo15 3:4abcd40682fa 34 // Controller gain proportional and intergrator
yohoo15 1:71617831bc4e 35 const double motor1_Kp = 5; // more or les random number.
yohoo15 8:998f4575089b 36 const double motor1_Ki = 0;
yohoo15 6:8ab07cce3098 37 const double M1_timestep = 0.01; // reason ticker works with 100 Hz.
yohoo15 3:4abcd40682fa 38 double motor1_error_integraal = 0; // initial value of integral error
yohoo15 0:2b481a8db14f 39
yohoo15 6:8ab07cce3098 40
yohoo15 7:6006a473ea0b 41 // Defining pulses per revolution (calculating pulses to rotations in degree.)
yohoo15 6:8ab07cce3098 42 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)
yohoo15 7:6006a473ea0b 43 /*
yohoo15 8:998f4575089b 44 double Rotation = -2; // rotation
yohoo15 6:8ab07cce3098 45 double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree.
yohoo15 7:6006a473ea0b 46 */
yohoo15 0:2b481a8db14f 47
yohoo15 5:d1ab07fd3355 48 // defining flags
yohoo15 5:d1ab07fd3355 49 volatile bool flag_motor = false;
yohoo15 5:d1ab07fd3355 50 volatile bool flag_pcprint = false;
yohoo15 0:2b481a8db14f 51
yohoo15 6:8ab07cce3098 52 // making function flags.
yohoo15 5:d1ab07fd3355 53 void Go_flag_motor()
yohoo15 5:d1ab07fd3355 54 {
yohoo15 5:d1ab07fd3355 55 flag_motor = true;
yohoo15 5:d1ab07fd3355 56 }
yohoo15 5:d1ab07fd3355 57
yohoo15 5:d1ab07fd3355 58 void Go_flag_pcprint()
yohoo15 5:d1ab07fd3355 59 {
yohoo15 5:d1ab07fd3355 60 flag_pcprint = true;
yohoo15 5:d1ab07fd3355 61 }
yohoo15 0:2b481a8db14f 62
yohoo15 7:6006a473ea0b 63 // To make a new setpoint
NickDGreg 9:ea9d35838861 64 double making_setpoint(double direction, double setpoint){
yohoo15 7:6006a473ea0b 65 if ( cw == direction){
NickDGreg 9:ea9d35838861 66 setpoint = setpoint + (pulses_per_revolution/400000);
yohoo15 7:6006a473ea0b 67 }
yohoo15 7:6006a473ea0b 68 if ( ccw == direction){
NickDGreg 9:ea9d35838861 69 setpoint = setpoint - (pulses_per_revolution/400000);
yohoo15 7:6006a473ea0b 70 }
yohoo15 7:6006a473ea0b 71 return(setpoint);
yohoo15 8:998f4575089b 72
yohoo15 7:6006a473ea0b 73 }
yohoo15 7:6006a473ea0b 74
yohoo15 0:2b481a8db14f 75 // Reusable P controller
yohoo15 3:4abcd40682fa 76 double PI(double error, const double Kp, const double Ki, double Ts, double &e_int)
yohoo15 6:8ab07cce3098 77 {
yohoo15 6:8ab07cce3098 78
yohoo15 6:8ab07cce3098 79 e_int = e_int + Ts * error;
yohoo15 6:8ab07cce3098 80
yohoo15 6:8ab07cce3098 81 double PI_output = (Kp * error) + (Ki * e_int);
yohoo15 3:4abcd40682fa 82 return PI_output;
yohoo15 0:2b481a8db14f 83 }
yohoo15 0:2b481a8db14f 84 // Next task, measure the error and apply the output to the plant
NickDGreg 9:ea9d35838861 85 void motor1_Controller(DigitalOut directionPin, PwmOut PWM, QEI &wheel, double setpoint)
yohoo15 0:2b481a8db14f 86 {
yohoo15 7:6006a473ea0b 87 double reference = setpoint; // setpoint is in pulses
yohoo15 0:2b481a8db14f 88 double position = wheel.getPulses();
yohoo15 6:8ab07cce3098 89 double error_pulses = (reference - position); // calculate the error in pulses
yohoo15 6:8ab07cce3098 90 double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn
yohoo15 6:8ab07cce3098 91
yohoo15 6:8ab07cce3098 92 double output = abs(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal ));
yohoo15 0:2b481a8db14f 93
yohoo15 6:8ab07cce3098 94 if(error_pulses > 0) {
yohoo15 0:2b481a8db14f 95 directionPin.write(cw);
yohoo15 6:8ab07cce3098 96
yohoo15 6:8ab07cce3098 97 }
yohoo15 6:8ab07cce3098 98 else if(error_pulses < 0) {
yohoo15 6:8ab07cce3098 99 directionPin.write(ccw);
yohoo15 0:2b481a8db14f 100 }
yohoo15 6:8ab07cce3098 101 else{
yohoo15 6:8ab07cce3098 102 output = 0;
yohoo15 6:8ab07cce3098 103 }
NickDGreg 10:3796b6a6c239 104
NickDGreg 10:3796b6a6c239 105 PWM.write(output); // out of the if loop due to abs output
yohoo15 0:2b481a8db14f 106
yohoo15 0:2b481a8db14f 107
yohoo15 0:2b481a8db14f 108 }
yohoo15 0:2b481a8db14f 109
yohoo15 0:2b481a8db14f 110
yohoo15 1:71617831bc4e 111 void counts_showing()
yohoo15 6:8ab07cce3098 112 {
yohoo15 1:71617831bc4e 113
NickDGreg 10:3796b6a6c239 114 //double kijken = wheel_one.getPulses()/pulses_per_revolution;
NickDGreg 10:3796b6a6c239 115 //pc.printf("ref %.2f rounds%.2f \n",setpoint_one/pulses_per_revolution,kijken);
NickDGreg 10:3796b6a6c239 116
NickDGreg 10:3796b6a6c239 117 double kijken = wheel_two.getPulses()/pulses_per_revolution;
NickDGreg 11:e2b2ea8a3be8 118 double pulses = wheel_two.getPulses();
NickDGreg 11:e2b2ea8a3be8 119 pc.printf("ref %.2f rounds%.2f pulses %.2f \n",setpoint_two/pulses_per_revolution,kijken, pulses);
yohoo15 6:8ab07cce3098 120
yohoo15 6:8ab07cce3098 121 }
yohoo15 0:2b481a8db14f 122
yohoo15 0:2b481a8db14f 123 int main()
yohoo15 0:2b481a8db14f 124 {
yohoo15 6:8ab07cce3098 125 aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning
yohoo15 5:d1ab07fd3355 126 Printen.attach(&Go_flag_pcprint,0.1f); // 10 Hz // printstatement here because printing cost to much time. the motor void wouldn't be completed
yohoo15 5:d1ab07fd3355 127 while( 1 ) {
yohoo15 7:6006a473ea0b 128 // to make the motor move
yohoo15 5:d1ab07fd3355 129 if(flag_motor) {
yohoo15 5:d1ab07fd3355 130 flag_motor = false;
NickDGreg 10:3796b6a6c239 131 //motor1_Controller(directionPin_one, PWM_one, wheel_one, setpoint_one);
NickDGreg 10:3796b6a6c239 132 motor1_Controller(directionPin_two, PWM_two, wheel_two, setpoint_two);
yohoo15 5:d1ab07fd3355 133 }
yohoo15 5:d1ab07fd3355 134
yohoo15 5:d1ab07fd3355 135 if(flag_pcprint) {
yohoo15 5:d1ab07fd3355 136 flag_pcprint = false;
yohoo15 5:d1ab07fd3355 137 counts_showing();
yohoo15 5:d1ab07fd3355 138 }
yohoo15 5:d1ab07fd3355 139
NickDGreg 11:e2b2ea8a3be8 140 // Pussing buttons to get input s0ignal
NickDGreg 11:e2b2ea8a3be8 141 /*
NickDGreg 11:e2b2ea8a3be8 142 ` while(buttoncw.read() == Buttoncw_pressed and buttonccw.read() == Buttonccw_pressed)
NickDGreg 11:e2b2ea8a3be8 143 {
NickDGreg 11:e2b2ea8a3be8 144 setpoint_two = making_setpoint(
NickDGreg 11:e2b2ea8a3be8 145 */
NickDGreg 10:3796b6a6c239 146 if(buttoncw.read() == Buttoncw_pressed)
NickDGreg 10:3796b6a6c239 147 {
NickDGreg 10:3796b6a6c239 148 //setpoint_one = making_setpoint(cw, setpoint_one);
NickDGreg 10:3796b6a6c239 149 setpoint_two = making_setpoint(ccw, setpoint_two);
yohoo15 7:6006a473ea0b 150 }
NickDGreg 10:3796b6a6c239 151
NickDGreg 10:3796b6a6c239 152 if(buttonccw.read() == Buttonccw_pressed)
NickDGreg 10:3796b6a6c239 153 {
NickDGreg 10:3796b6a6c239 154 //setpoint_one = making_setpoint(ccw, setpoint_one);
NickDGreg 10:3796b6a6c239 155 setpoint_two = making_setpoint(cw, setpoint_two);
yohoo15 7:6006a473ea0b 156 }
yohoo15 7:6006a473ea0b 157
yohoo15 5:d1ab07fd3355 158 }
yohoo15 0:2b481a8db14f 159 }
yohoo15 0:2b481a8db14f 160
yohoo15 0:2b481a8db14f 161
yohoo15 0:2b481a8db14f 162