Jona Vonk / Mbed OS MoveMotors

Hier zitten extra leipe functies in naast PID ook x en y naar hoeken

Dependencies:   QEI MODSERIAL

main.cpp@2:f832050b1b4a, 2019-10-21 (annotated)

Committer:
JonaVonk
Date:
Mon Oct 21 12:28:08 2019 +0000
Revision:
2:f832050b1b4a
Parent:
1:363c5230fe25
Child:
3:ecd394ae8118
Leipe dingen gemaakt door een epische programmeur.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JonaVonk 0:6986a58c4515 1 /* mbed Microcontroller Library
JonaVonk 0:6986a58c4515 2 * Copyright (c) 2018 ARM Limited
JonaVonk 0:6986a58c4515 3 * SPDX-License-Identifier: Apache-2.0
JonaVonk 0:6986a58c4515 4 */
JonaVonk 0:6986a58c4515 5
JonaVonk 0:6986a58c4515 6 #include "mbed.h"
JonaVonk 0:6986a58c4515 7 #include "MODSERIAL.h"
JonaVonk 2:f832050b1b4a 8 #include "QEI.h"
JonaVonk 0:6986a58c4515 9
JonaVonk 2:f832050b1b4a 10 QEI Encoder(D12,D13,NC,32);
JonaVonk 2:f832050b1b4a 11
JonaVonk 2:f832050b1b4a 12 DigitalOut M1(D4);
JonaVonk 2:f832050b1b4a 13 DigitalOut M2(D7);
JonaVonk 0:6986a58c4515 14
JonaVonk 2:f832050b1b4a 15 PwmOut E1(D5);
JonaVonk 2:f832050b1b4a 16 PwmOut E2(D6);
JonaVonk 2:f832050b1b4a 17
JonaVonk 2:f832050b1b4a 18 AnalogIn Pot1(A0);
JonaVonk 2:f832050b1b4a 19 AnalogIn Pot2(A1);
JonaVonk 0:6986a58c4515 20
JonaVonk 2:f832050b1b4a 21 float potVal1;
JonaVonk 2:f832050b1b4a 22 float potVal2;
JonaVonk 0:6986a58c4515 23
JonaVonk 2:f832050b1b4a 24 float pi = 3.14159265359;
JonaVonk 2:f832050b1b4a 25
JonaVonk 0:6986a58c4515 26
JonaVonk 0:6986a58c4515 27 MODSERIAL pc(USBTX, USBRX);
JonaVonk 0:6986a58c4515 28
JonaVonk 2:f832050b1b4a 29
JonaVonk 2:f832050b1b4a 30 void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes);
JonaVonk 0:6986a58c4515 31
JonaVonk 0:6986a58c4515 32
JonaVonk 0:6986a58c4515 33 // main() runs in its own thread in the OS
JonaVonk 2:f832050b1b4a 34 int main()
JonaVonk 2:f832050b1b4a 35 {
JonaVonk 2:f832050b1b4a 36 float steps = 100;
JonaVonk 2:f832050b1b4a 37 pc.baud(115200);
JonaVonk 2:f832050b1b4a 38 while (true){
JonaVonk 2:f832050b1b4a 39 for(int i = 0; i < steps; i++){
JonaVonk 2:f832050b1b4a 40 moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
JonaVonk 2:f832050b1b4a 41 //pc.printf("step: %f\n\r", float(i)/steps);
JonaVonk 0:6986a58c4515 42 }
JonaVonk 2:f832050b1b4a 43 for(int i = steps; i > 0; i--){
JonaVonk 2:f832050b1b4a 44 moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
JonaVonk 2:f832050b1b4a 45 }
JonaVonk 0:6986a58c4515 46 }
JonaVonk 0:6986a58c4515 47 }
JonaVonk 2:f832050b1b4a 48
JonaVonk 2:f832050b1b4a 49 void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes)
JonaVonk 2:f832050b1b4a 50 {
JonaVonk 2:f832050b1b4a 51 float pErrorC;
JonaVonk 2:f832050b1b4a 52 float pErrorP = 0;
JonaVonk 2:f832050b1b4a 53 float iError = 0;
JonaVonk 2:f832050b1b4a 54 float dError;
JonaVonk 2:f832050b1b4a 55
JonaVonk 2:f832050b1b4a 56 float Kp = 5;
JonaVonk 2:f832050b1b4a 57 float Ki = 0.01;
JonaVonk 2:f832050b1b4a 58 float Kd = 0.7;
JonaVonk 2:f832050b1b4a 59
JonaVonk 2:f832050b1b4a 60 float rotC = Enc->getPulses()/(32*131.25);
JonaVonk 2:f832050b1b4a 61 float rotP = 0;
JonaVonk 2:f832050b1b4a 62 float MotorPWM;
JonaVonk 2:f832050b1b4a 63
JonaVonk 2:f832050b1b4a 64 Timer t;
JonaVonk 2:f832050b1b4a 65 float tieme = 0;
JonaVonk 2:f832050b1b4a 66
JonaVonk 2:f832050b1b4a 67 t.start();
JonaVonk 2:f832050b1b4a 68 do {
JonaVonk 2:f832050b1b4a 69 pErrorP = pErrorC;
JonaVonk 2:f832050b1b4a 70 pErrorC = rotDes - rotC;
JonaVonk 2:f832050b1b4a 71 iError = iError + pErrorC*tieme;
JonaVonk 2:f832050b1b4a 72 dError = (pErrorC - pErrorP)/tieme;
JonaVonk 2:f832050b1b4a 73
JonaVonk 2:f832050b1b4a 74 MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;
JonaVonk 2:f832050b1b4a 75
JonaVonk 2:f832050b1b4a 76 if(MotorPWM > 0) {
JonaVonk 2:f832050b1b4a 77 *M = 0;
JonaVonk 2:f832050b1b4a 78 *E = MotorPWM;
JonaVonk 2:f832050b1b4a 79 } else {
JonaVonk 2:f832050b1b4a 80 *M = 1;
JonaVonk 2:f832050b1b4a 81 *E = -MotorPWM;
JonaVonk 2:f832050b1b4a 82 }
JonaVonk 2:f832050b1b4a 83
JonaVonk 2:f832050b1b4a 84 rotP = rotC;
JonaVonk 2:f832050b1b4a 85 rotC = Enc->getPulses()/(32*131.25);
JonaVonk 2:f832050b1b4a 86 tieme = t.read();
JonaVonk 2:f832050b1b4a 87 t.reset();
JonaVonk 2:f832050b1b4a 88 //pc.printf("pError: %f dError: %f iError: %f PWM: %f\n\r", pErrorC, dError, iError, MotorPWM);
JonaVonk 2:f832050b1b4a 89 } while (pErrorC > 0.01 || pErrorC < -0.01 ||dError > 0.01 || dError < -0.01);
JonaVonk 2:f832050b1b4a 90 //*E = 0;
JonaVonk 2:f832050b1b4a 91 t.stop();
JonaVonk 2:f832050b1b4a 92 }
JonaVonk 2:f832050b1b4a 93