groep 16 / Mbed 2 deprecated Calibration_mode

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 HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@7:13bb9bf83f58, 2019-10-31 (annotated)

Committer:
JonaVonk
Date:
Thu Oct 31 11:32:08 2019 +0000
Revision:
7:13bb9bf83f58
Parent:
6:105b306350c6
Child:
8:ce83823803a3
lekker calibreren;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
RobertoO 0:67c50348f842 2 //#include "HIDScope.h"
JonaVonk 2:96e093a48314 3 #include "QEI.h"
RobertoO 1:b862262a9d14 4 #include "MODSERIAL.h"
RobertoO 0:67c50348f842 5 //#include "BiQuad.h"
RobertoO 1:b862262a9d14 6 //#include "FastPWM.h"
JonaVonk 2:96e093a48314 7 #include <vector>
RobertoO 0:67c50348f842 8
JonaVonk 2:96e093a48314 9 using std::vector;
JonaVonk 2:96e093a48314 10
JonaVonk 2:96e093a48314 11 double Pi = 3.14159265359;
JonaVonk 2:96e093a48314 12
JonaVonk 2:96e093a48314 13 QEI Enc1(D12, D13, NC, 32);
JonaVonk 2:96e093a48314 14 QEI Enc2(D10, D11, NC, 32);
JonaVonk 2:96e093a48314 15
JonaVonk 2:96e093a48314 16 DigitalOut M1(D4);
JonaVonk 2:96e093a48314 17 DigitalOut M2(D7);
JonaVonk 2:96e093a48314 18
JonaVonk 2:96e093a48314 19 PwmOut E1(D5);
JonaVonk 2:96e093a48314 20 PwmOut E2(D6);
JonaVonk 2:96e093a48314 21
JonaVonk 7:13bb9bf83f58 22 double gearRatio = 40/9;
JonaVonk 7:13bb9bf83f58 23
JonaVonk 2:96e093a48314 24 double initRot1 = 0;
JonaVonk 7:13bb9bf83f58 25 double initRot2 = -gearRatio*(180 - 48.5)/360;
JonaVonk 2:96e093a48314 26
RobertoO 0:67c50348f842 27
RobertoO 1:b862262a9d14 28 MODSERIAL pc(USBTX, USBRX);
RobertoO 0:67c50348f842 29
JonaVonk 7:13bb9bf83f58 30 void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double MotorPWM);
JonaVonk 7:13bb9bf83f58 31 void moveMotorsToStop(DigitalOut *M1, PwmOut *E1, QEI *Enc1, double speed1, DigitalOut *M2, PwmOut *E2, QEI *Enc2, double speed2);
JonaVonk 7:13bb9bf83f58 32 void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes);
JonaVonk 2:96e093a48314 33
JonaVonk 2:96e093a48314 34 // main() runs in its own thread in the OS
RobertoO 0:67c50348f842 35 int main()
RobertoO 0:67c50348f842 36 {
RobertoO 0:67c50348f842 37 pc.baud(115200);
JonaVonk 2:96e093a48314 38 pc.printf("Start\n\r");
JonaVonk 7:13bb9bf83f58 39 moveMotorToStop(&M1, &E1, &Enc1, -0.1);
JonaVonk 7:13bb9bf83f58 40 Enc1.reset();
JonaVonk 7:13bb9bf83f58 41 moveMotorToPoint(&M1, &E1, &Enc1, 0, 1, 0.2);
JonaVonk 7:13bb9bf83f58 42 moveMotorsToStop(&M2, &E2, &Enc2, 0.05, &M1, &E1, &Enc1, 0.3);
JonaVonk 7:13bb9bf83f58 43 pc.printf("end\n\r");
JonaVonk 2:96e093a48314 44 }
JonaVonk 2:96e093a48314 45
JonaVonk 2:96e093a48314 46
JonaVonk 7:13bb9bf83f58 47 void moveMotorsToStop(DigitalOut *M1, PwmOut *E1, QEI *Enc1, double speed1, DigitalOut *M2, PwmOut *E2, QEI *Enc2, double speed2)
JonaVonk 2:96e093a48314 48 {
JonaVonk 7:13bb9bf83f58 49 Timer t;
JonaVonk 7:13bb9bf83f58 50
JonaVonk 7:13bb9bf83f58 51 double posC1;
JonaVonk 7:13bb9bf83f58 52 double posP1 = Enc1->getPulses()/(32*131.25);
JonaVonk 7:13bb9bf83f58 53 double vel1 = 0;
JonaVonk 7:13bb9bf83f58 54 double MotorPWM1;
JonaVonk 7:13bb9bf83f58 55
JonaVonk 7:13bb9bf83f58 56 double posC2;
JonaVonk 7:13bb9bf83f58 57 double posP2 = Enc2->getPulses()/(32*131.25);
JonaVonk 7:13bb9bf83f58 58 double vel2 = 0;
JonaVonk 7:13bb9bf83f58 59 double MotorPWM2;
JonaVonk 7:13bb9bf83f58 60
JonaVonk 7:13bb9bf83f58 61 int hasNotMoved = 0;
JonaVonk 7:13bb9bf83f58 62
JonaVonk 7:13bb9bf83f58 63
JonaVonk 7:13bb9bf83f58 64 t.start();
JonaVonk 7:13bb9bf83f58 65 do {
JonaVonk 7:13bb9bf83f58 66 MotorPWM1 = speed1 - vel1*0.5;
JonaVonk 7:13bb9bf83f58 67 if(MotorPWM1 > 0) {
JonaVonk 7:13bb9bf83f58 68 *M1 = 0;
JonaVonk 7:13bb9bf83f58 69 *E1 = MotorPWM1;
JonaVonk 7:13bb9bf83f58 70 } else {
JonaVonk 7:13bb9bf83f58 71 *M1 = 1;
JonaVonk 7:13bb9bf83f58 72 *E1 = -MotorPWM1;
JonaVonk 7:13bb9bf83f58 73 }
JonaVonk 7:13bb9bf83f58 74 MotorPWM2 = speed2 - vel2*0.5;
JonaVonk 7:13bb9bf83f58 75 if(MotorPWM2 > 0) {
JonaVonk 7:13bb9bf83f58 76 *M2 = 0;
JonaVonk 7:13bb9bf83f58 77 *E2 = MotorPWM2;
JonaVonk 7:13bb9bf83f58 78 } else {
JonaVonk 7:13bb9bf83f58 79 *M2 = 1;
JonaVonk 7:13bb9bf83f58 80 *E2 = -MotorPWM2;
JonaVonk 7:13bb9bf83f58 81 }
JonaVonk 7:13bb9bf83f58 82 wait(0.01);
JonaVonk 7:13bb9bf83f58 83 posC1 = Enc1->getPulses()/(32*131.25);
JonaVonk 7:13bb9bf83f58 84 vel1 = (posC1 - posP1)/t.read();
JonaVonk 7:13bb9bf83f58 85 posP1 = posC1;
JonaVonk 7:13bb9bf83f58 86 //pc.printf("v: %d hm: %d\n\r", (abs(vel) > 0.001), hasNotMoved);
JonaVonk 7:13bb9bf83f58 87 posC2 = Enc2->getPulses()/(32*131.25);
JonaVonk 7:13bb9bf83f58 88 vel2 = (posC2 - posP2)/t.read();
JonaVonk 7:13bb9bf83f58 89 t.reset();
JonaVonk 7:13bb9bf83f58 90 posP2 = posC2;
JonaVonk 7:13bb9bf83f58 91 //pc.printf("v: %d hm: %d\n\r", (abs(vel) > 0.001), hasNotMoved);
JonaVonk 7:13bb9bf83f58 92 if(abs(vel1) > 0.001) {
JonaVonk 7:13bb9bf83f58 93 hasNotMoved = 0;
JonaVonk 7:13bb9bf83f58 94 } else {
JonaVonk 7:13bb9bf83f58 95 hasNotMoved++;
JonaVonk 7:13bb9bf83f58 96 }
JonaVonk 7:13bb9bf83f58 97 } while(abs(vel1) > 0.001 || hasNotMoved < 10);
JonaVonk 7:13bb9bf83f58 98 *E1 = 0;
JonaVonk 7:13bb9bf83f58 99 *E2 = 0;
JonaVonk 7:13bb9bf83f58 100 }
JonaVonk 7:13bb9bf83f58 101
JonaVonk 7:13bb9bf83f58 102 void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed)
JonaVonk 7:13bb9bf83f58 103 {
JonaVonk 7:13bb9bf83f58 104 Timer t;
JonaVonk 7:13bb9bf83f58 105
JonaVonk 7:13bb9bf83f58 106 double MotorPWM;
JonaVonk 7:13bb9bf83f58 107
JonaVonk 7:13bb9bf83f58 108 double posC;
JonaVonk 7:13bb9bf83f58 109 double posP = Enc->getPulses()/(32*131.25);
JonaVonk 7:13bb9bf83f58 110 double vel = 0;
JonaVonk 7:13bb9bf83f58 111
JonaVonk 7:13bb9bf83f58 112 int hasNotMoved = 0;
JonaVonk 7:13bb9bf83f58 113
JonaVonk 7:13bb9bf83f58 114
JonaVonk 7:13bb9bf83f58 115 t.start();
JonaVonk 7:13bb9bf83f58 116 do {
JonaVonk 7:13bb9bf83f58 117 MotorPWM = speed - vel*0.5;
JonaVonk 7:13bb9bf83f58 118 if(MotorPWM > 0) {
JonaVonk 7:13bb9bf83f58 119 *M = 0;
JonaVonk 7:13bb9bf83f58 120 *E = MotorPWM;
JonaVonk 7:13bb9bf83f58 121 } else {
JonaVonk 7:13bb9bf83f58 122 *M = 1;
JonaVonk 7:13bb9bf83f58 123 *E = -MotorPWM;
JonaVonk 7:13bb9bf83f58 124 }
JonaVonk 7:13bb9bf83f58 125 wait(0.01);
JonaVonk 7:13bb9bf83f58 126 posC = Enc->getPulses()/(32*131.25);
JonaVonk 7:13bb9bf83f58 127 vel = (posC - posP)/t.read();
JonaVonk 7:13bb9bf83f58 128 t.reset();
JonaVonk 7:13bb9bf83f58 129 posP = posC;
JonaVonk 7:13bb9bf83f58 130 pc.printf("v: %d hm: %d\n\r", (abs(vel) > 0.001), hasNotMoved);
JonaVonk 7:13bb9bf83f58 131 if(abs(vel) > 0.001) {
JonaVonk 7:13bb9bf83f58 132 hasNotMoved = 0;
JonaVonk 7:13bb9bf83f58 133 } else {
JonaVonk 7:13bb9bf83f58 134 hasNotMoved++;
JonaVonk 7:13bb9bf83f58 135 }
JonaVonk 7:13bb9bf83f58 136 } while(abs(vel) > 0.001 || hasNotMoved < 10);
JonaVonk 7:13bb9bf83f58 137 *E = 0;
JonaVonk 7:13bb9bf83f58 138 }
JonaVonk 7:13bb9bf83f58 139
JonaVonk 7:13bb9bf83f58 140 void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
JonaVonk 7:13bb9bf83f58 141 {
JonaVonk 7:13bb9bf83f58 142 double Kp = 30; //180 & 10 werkt zonder hulp
JonaVonk 7:13bb9bf83f58 143 double Ki = 0;
JonaVonk 7:13bb9bf83f58 144 double Kd = 2;
JonaVonk 7:13bb9bf83f58 145
JonaVonk 5:5082d694e643 146 double pErrorC;
JonaVonk 5:5082d694e643 147 double pErrorP = 0;
JonaVonk 5:5082d694e643 148 double iError = 0;
JonaVonk 5:5082d694e643 149 double dError;
JonaVonk 2:96e093a48314 150
JonaVonk 5:5082d694e643 151 double U_p;
JonaVonk 5:5082d694e643 152 double U_i;
JonaVonk 5:5082d694e643 153 double U_d;
JonaVonk 2:96e093a48314 154
JonaVonk 5:5082d694e643 155 double rotC = Enc->getPulses()/(32*131.25) + initRot;
JonaVonk 5:5082d694e643 156 double MotorPWM;
JonaVonk 2:96e093a48314 157
JonaVonk 2:96e093a48314 158 Timer t;
JonaVonk 5:5082d694e643 159 double tieme = 0;
JonaVonk 2:96e093a48314 160
JonaVonk 2:96e093a48314 161 t.start();
JonaVonk 2:96e093a48314 162 do {
JonaVonk 2:96e093a48314 163 pErrorP = pErrorC;
JonaVonk 5:5082d694e643 164 rotC = Enc->getPulses()/(32*131.25) + initRot;
JonaVonk 2:96e093a48314 165 pErrorC = rotDes - rotC;
JonaVonk 5:5082d694e643 166 tieme = t.read();
JonaVonk 5:5082d694e643 167 t.reset();
JonaVonk 2:96e093a48314 168 iError = iError + pErrorC*tieme;
JonaVonk 2:96e093a48314 169 dError = (pErrorC - pErrorP)/tieme;
JonaVonk 7:13bb9bf83f58 170
JonaVonk 5:5082d694e643 171 U_p = pErrorC*Kp;
JonaVonk 5:5082d694e643 172 U_i = iError*Ki;
JonaVonk 5:5082d694e643 173 U_d = dError*Kd;
JonaVonk 5:5082d694e643 174 MotorPWM = (U_p + U_i + U_d)*dir;
JonaVonk 2:96e093a48314 175
JonaVonk 2:96e093a48314 176 if(MotorPWM > 0) {
JonaVonk 2:96e093a48314 177 *M = 0;
JonaVonk 2:96e093a48314 178 *E = MotorPWM;
JonaVonk 2:96e093a48314 179 } else {
JonaVonk 2:96e093a48314 180 *M = 1;
JonaVonk 2:96e093a48314 181 *E = -MotorPWM;
JonaVonk 2:96e093a48314 182 }
JonaVonk 6:105b306350c6 183 wait(0.01);
JonaVonk 7:13bb9bf83f58 184 printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
JonaVonk 7:13bb9bf83f58 185 } while (abs(MotorPWM) > 0.13|| abs(dError > 0.01));; //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01);
JonaVonk 5:5082d694e643 186 *E = 0;
JonaVonk 6:105b306350c6 187 //pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
JonaVonk 2:96e093a48314 188 t.stop();
JonaVonk 7:13bb9bf83f58 189 }