Astrid Schut / Mbed 2 deprecated PWMmotor

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI MODSERIAL FastPWM biquadFilter

main.cpp@6:14a9c4f30c86, 2019-03-19 (annotated)

Committer:
aschut
Date:
Tue Mar 19 16:12:42 2019 +0000
Revision:
6:14a9c4f30c86
Parent:
5:4b25551aeb6e
Child:
7:9a1007e35bac
Geschikt voor Matlab

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aschut 0:a9a42914138c 1 #include "mbed.h"
aschut 2:926d56babb1a 2 #include "MODSERIAL.h"
aschut 4:99f7fdce608e 3 #include "QEI.h"
aschut 6:14a9c4f30c86 4 #include "BiQuad.h"
aschut 4:99f7fdce608e 5
aschut 4:99f7fdce608e 6 // Algemeen
aschut 1:58f34947c674 7 DigitalIn button2(SW2);
aschut 1:58f34947c674 8 DigitalIn button3(SW3);
aschut 2:926d56babb1a 9 DigitalOut led(LED_GREEN);
aschut 2:926d56babb1a 10 DigitalOut led2(LED_RED);
aschut 4:99f7fdce608e 11 DigitalOut led3(LED_BLUE);
aschut 2:926d56babb1a 12 MODSERIAL pc(USBTX, USBRX);
aschut 2:926d56babb1a 13
aschut 4:99f7fdce608e 14 //Motoren
aschut 0:a9a42914138c 15 DigitalOut direction1(D4);
aschut 0:a9a42914138c 16 PwmOut pwmpin1(D5);
aschut 0:a9a42914138c 17 PwmOut pwmpin2(D6);
aschut 0:a9a42914138c 18 DigitalOut direction2(D7);
aschut 4:99f7fdce608e 19 volatile float PWM1;
aschut 4:99f7fdce608e 20 volatile float PWM2;
aschut 4:99f7fdce608e 21 volatile float pwm2;
aschut 4:99f7fdce608e 22
aschut 4:99f7fdce608e 23 //Encoder
aschut 4:99f7fdce608e 24 DigitalIn EncoderA(D13);
aschut 4:99f7fdce608e 25 DigitalIn EncoderB(D12);
aschut 4:99f7fdce608e 26 QEI encoder2 (D13, D12, NC, 8400, QEI::X4_ENCODING);
aschut 6:14a9c4f30c86 27 double Pulses2;
aschut 6:14a9c4f30c86 28 double motor_position2;
aschut 2:926d56babb1a 29
aschut 2:926d56babb1a 30 //Pot meter
aschut 1:58f34947c674 31 AnalogIn pot(A1);
aschut 6:14a9c4f30c86 32 AnalogIn pot0(A0);
aschut 4:99f7fdce608e 33 float Pot2;
aschut 6:14a9c4f30c86 34 float Pot1;
aschut 4:99f7fdce608e 35
aschut 4:99f7fdce608e 36 //Ticker
aschut 4:99f7fdce608e 37 Ticker Pwm;
aschut 2:926d56babb1a 38 Ticker PotRead;
aschut 6:14a9c4f30c86 39
aschut 0:a9a42914138c 40
aschut 5:4b25551aeb6e 41 //Kinematica
aschut 6:14a9c4f30c86 42 double stap;
aschut 6:14a9c4f30c86 43 double KPot;
aschut 5:4b25551aeb6e 44 float KPotabs;
aschut 5:4b25551aeb6e 45 float ElbowReference;
aschut 5:4b25551aeb6e 46 float Ellebooghoek1;
aschut 5:4b25551aeb6e 47 float Ellebooghoek2;
aschut 5:4b25551aeb6e 48 float Ellebooghoek3;
aschut 5:4b25551aeb6e 49 float Ellebooghoek4;
aschut 5:4b25551aeb6e 50 float Hoeknieuw;
aschut 5:4b25551aeb6e 51
aschut 5:4b25551aeb6e 52 //Limiet in graden
aschut 5:4b25551aeb6e 53 float lowerlim = 0;
aschut 5:4b25551aeb6e 54 float upperlim = 748.8; //40% van 1 ronde van het grote tandwiel is 2,08 rondes van de motor
aschut 2:926d56babb1a 55
aschut 6:14a9c4f30c86 56 // VARIABLES PID CONTROLLER
aschut 6:14a9c4f30c86 57 double Kp = Pot1*10; //
aschut 6:14a9c4f30c86 58 double Ki = 5; //
aschut 6:14a9c4f30c86 59 double Kd = 5; //
aschut 6:14a9c4f30c86 60 double Ts = 0.1; // Sample time in seconds
aschut 6:14a9c4f30c86 61
aschut 6:14a9c4f30c86 62
aschut 6:14a9c4f30c86 63
aschut 6:14a9c4f30c86 64 }
aschut 5:4b25551aeb6e 65 float Kinematics(float KPot)
aschut 6:14a9c4f30c86 66 {
aschut 6:14a9c4f30c86 67
aschut 6:14a9c4f30c86 68 if (KPot > 0.45f) {
aschut 6:14a9c4f30c86 69 stap = KPot*150*Ts; // 144 graden van de arm in 5 seconden
aschut 6:14a9c4f30c86 70 Hoeknieuw = ElbowReference + stap;
aschut 6:14a9c4f30c86 71 return Hoeknieuw;
aschut 5:4b25551aeb6e 72 }
aschut 4:99f7fdce608e 73
aschut 6:14a9c4f30c86 74 else if (KPot < -0.45f) {
aschut 6:14a9c4f30c86 75 stap = KPot*150*Ts;
aschut 6:14a9c4f30c86 76 Hoeknieuw = ElbowReference + stap;
aschut 6:14a9c4f30c86 77 return Hoeknieuw;
aschut 6:14a9c4f30c86 78 }
aschut 6:14a9c4f30c86 79
aschut 6:14a9c4f30c86 80 else {
aschut 6:14a9c4f30c86 81 return ElbowReference;
aschut 6:14a9c4f30c86 82 }
aschut 6:14a9c4f30c86 83 }
aschut 6:14a9c4f30c86 84
aschut 6:14a9c4f30c86 85 float Limits(float Ellebooghoek2)
aschut 6:14a9c4f30c86 86 {
aschut 6:14a9c4f30c86 87
aschut 5:4b25551aeb6e 88 if (Ellebooghoek2 <= upperlim && Ellebooghoek2 >= lowerlim) { //Binnen de limieten
aschut 5:4b25551aeb6e 89 Ellebooghoek3 = Ellebooghoek2;
aschut 6:14a9c4f30c86 90 }
aschut 6:14a9c4f30c86 91
aschut 5:4b25551aeb6e 92 else {
aschut 6:14a9c4f30c86 93 if (Ellebooghoek2 >= upperlim) { //Boven de limiet
aschut 5:4b25551aeb6e 94 Ellebooghoek3 = upperlim;
aschut 6:14a9c4f30c86 95 } else { //Onder de limiet
aschut 5:4b25551aeb6e 96 Ellebooghoek3 = lowerlim;
aschut 5:4b25551aeb6e 97 }
aschut 5:4b25551aeb6e 98 }
aschut 6:14a9c4f30c86 99
aschut 5:4b25551aeb6e 100 return Ellebooghoek3;
aschut 6:14a9c4f30c86 101 }
aschut 6:14a9c4f30c86 102
aschut 6:14a9c4f30c86 103
aschut 4:99f7fdce608e 104
aschut 0:a9a42914138c 105
aschut 6:14a9c4f30c86 106 // ~~~~~~~~~~~~~~PID CONTROLLER~~~~~~~~~~~~~~~~~~
aschut 6:14a9c4f30c86 107
aschut 6:14a9c4f30c86 108 double PID_controller(double error)
aschut 6:14a9c4f30c86 109 {
aschut 6:14a9c4f30c86 110 static double error_integral = 0;
aschut 6:14a9c4f30c86 111 static double error_prev = error; // initialization with this value only done once!
aschut 6:14a9c4f30c86 112 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
aschut 6:14a9c4f30c86 113
aschut 6:14a9c4f30c86 114 // Proportional part:
aschut 6:14a9c4f30c86 115 double u_k = Kp * error;
aschut 6:14a9c4f30c86 116
aschut 6:14a9c4f30c86 117 // Integral part
aschut 6:14a9c4f30c86 118 error_integral = error_integral + error * Ts;
aschut 6:14a9c4f30c86 119 double u_i = Ki * error_integral;
aschut 6:14a9c4f30c86 120
aschut 6:14a9c4f30c86 121 // Derivative part
aschut 6:14a9c4f30c86 122 double error_derivative = (error - error_prev)/Ts;
aschut 6:14a9c4f30c86 123 double filtered_error_derivative = LowPassFilter.step(error_derivative);
aschut 6:14a9c4f30c86 124 double u_d = Kd * filtered_error_derivative;
aschut 6:14a9c4f30c86 125 error_prev = error;
aschut 6:14a9c4f30c86 126
aschut 6:14a9c4f30c86 127 // Sum all parts and return it
aschut 6:14a9c4f30c86 128 return u_k + u_i + u_d;
aschut 6:14a9c4f30c86 129 }
aschut 6:14a9c4f30c86 130
aschut 6:14a9c4f30c86 131 void moter2_control(double u)
aschut 6:14a9c4f30c86 132 {
aschut 6:14a9c4f30c86 133 direction2= u < 0.0f; //positief = CW
aschut 6:14a9c4f30c86 134 if (fabs(u)> 0.7f) {
aschut 6:14a9c4f30c86 135 u = 0.7f;
aschut 6:14a9c4f30c86 136 } else {
aschut 6:14a9c4f30c86 137 u= u;
aschut 0:a9a42914138c 138 }
aschut 6:14a9c4f30c86 139 pwmpin2= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
aschut 6:14a9c4f30c86 140 }
aschut 6:14a9c4f30c86 141
aschut 4:99f7fdce608e 142 void PwmMotor(void)
aschut 6:14a9c4f30c86 143 {
aschut 5:4b25551aeb6e 144 float Ellebooghoek1 = Kinematics(pwm2);
aschut 5:4b25551aeb6e 145 float Ellebooghoek4 = Limits(Ellebooghoek1);
aschut 5:4b25551aeb6e 146 ElbowReference = Ellebooghoek4;
aschut 6:14a9c4f30c86 147
aschut 6:14a9c4f30c86 148 //pc.printf("ElbowReference:%f\n", ElbowReference);
aschut 6:14a9c4f30c86 149 //pc.printf("pwm2=%f\r\n",pwm2);
aschut 6:14a9c4f30c86 150
aschut 6:14a9c4f30c86 151 Pulses2 = encoder2.getPulses();
aschut 6:14a9c4f30c86 152 motor_position2 = -(Pulses2/8400)*360;
aschut 4:99f7fdce608e 153
aschut 6:14a9c4f30c86 154 double error = reference_rotation - motor_position2;
aschut 6:14a9c4f30c86 155 //double error = ElbowReference - motor_position2;
aschut 6:14a9c4f30c86 156 //pc.printf("Degrees is:%f, error = %f\n", motor_position2, error);
aschut 2:926d56babb1a 157
aschut 6:14a9c4f30c86 158 //pwmpin2 = fabs(pwm2); //zonder PID
aschut 6:14a9c4f30c86 159
aschut 6:14a9c4f30c86 160 double u = PID_controller(error);
aschut 6:14a9c4f30c86 161 moter2_control(u);
aschut 6:14a9c4f30c86 162
aschut 6:14a9c4f30c86 163 }
aschut 6:14a9c4f30c86 164
aschut 1:58f34947c674 165 void MotorOn(void)
aschut 6:14a9c4f30c86 166 {
aschut 4:99f7fdce608e 167 pwmpin2 = 0;
aschut 6:14a9c4f30c86 168 Pwm.attach (PwmMotor, Ts);
aschut 6:14a9c4f30c86 169
aschut 6:14a9c4f30c86 170 }
aschut 1:58f34947c674 171 void MotorOff(void)
aschut 6:14a9c4f30c86 172 {
aschut 6:14a9c4f30c86 173 Pwm.detach ();
aschut 1:58f34947c674 174 pwmpin2 = 0;
aschut 6:14a9c4f30c86 175 }
aschut 4:99f7fdce608e 176
aschut 6:14a9c4f30c86 177 void ContinuousReader(void)
aschut 6:14a9c4f30c86 178 {
aschut 3:ac13255164cd 179 Pot2 = pot.read();
aschut 6:14a9c4f30c86 180 Pot1 = pot0.read();
aschut 5:4b25551aeb6e 181 pwm2 =(Pot2*2)-1; //scaling
aschut 6:14a9c4f30c86 182
aschut 4:99f7fdce608e 183 //pc.printf("%f\r\n",Pot2);
aschut 6:14a9c4f30c86 184 }
aschut 6:14a9c4f30c86 185
aschut 6:14a9c4f30c86 186
aschut 1:58f34947c674 187
aschut 6:14a9c4f30c86 188 int main()
aschut 6:14a9c4f30c86 189 {
aschut 6:14a9c4f30c86 190 Timer t;
aschut 6:14a9c4f30c86 191 t.start();
aschut 6:14a9c4f30c86 192 int counter = 0;
aschut 6:14a9c4f30c86 193 pwmpin2.period_us(60);
aschut 6:14a9c4f30c86 194 PotRead.attach(ContinuousReader,Ts);
aschut 2:926d56babb1a 195 pc.baud(115200);
aschut 6:14a9c4f30c86 196 //pc.printf("start\r\n");
aschut 4:99f7fdce608e 197 led = 1;
aschut 4:99f7fdce608e 198 led2 =1;
aschut 4:99f7fdce608e 199 led3 =1;
aschut 6:14a9c4f30c86 200
aschut 6:14a9c4f30c86 201 while (true) {
aschut 6:14a9c4f30c86 202 led3 = 0;
aschut 6:14a9c4f30c86 203 if (!button2) {
aschut 6:14a9c4f30c86 204 led3 = 1;
aschut 6:14a9c4f30c86 205 led = 0;
aschut 6:14a9c4f30c86 206 //pc.printf("MotorOn\r\n");
aschut 6:14a9c4f30c86 207 MotorOn();
aschut 6:14a9c4f30c86 208 }
aschut 6:14a9c4f30c86 209 if (!button3) {
aschut 6:14a9c4f30c86 210 //pc.printf("MotorOff\r\n");
aschut 6:14a9c4f30c86 211 PotRead.detach();
aschut 6:14a9c4f30c86 212 MotorOff();
aschut 6:14a9c4f30c86 213 }
aschut 4:99f7fdce608e 214 led = 0;
aschut 6:14a9c4f30c86 215 if(counter==10) {
aschut 6:14a9c4f30c86 216 float tmp = t.read();
aschut 6:14a9c4f30c86 217 printf("%f,%f,%f,%f\n\r",tmp,motor_position2,reference_rotation,Pot1);
aschut 6:14a9c4f30c86 218 counter = 0;
aschut 6:14a9c4f30c86 219 }
aschut 6:14a9c4f30c86 220 counter++;
aschut 6:14a9c4f30c86 221 wait(0.001);
aschut 1:58f34947c674 222 }
aschut 6:14a9c4f30c86 223 }
aschut 0:a9a42914138c 224
aschut 0:a9a42914138c 225
aschut 6:14a9c4f30c86 226