groep 16 / Mbed OS MoveMotors

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:   QEI MODSERIAL

main.cpp@4:5f147787c2ca, 2019-10-24 (annotated)

Committer:
JonaVonk
Date:
Thu Oct 24 11:46:00 2019 +0000
Revision:
4:5f147787c2ca
Parent:
3:ecd394ae8118 
Hier zitten mega advanced functies bij

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 4:5f147787c2ca 9 #include "math.h"
JonaVonk 0:6986a58c4515 10
JonaVonk 2:f832050b1b4a 11 QEI Encoder(D12,D13,NC,32);
JonaVonk 2:f832050b1b4a 12
JonaVonk 2:f832050b1b4a 13 DigitalOut M1(D4);
JonaVonk 2:f832050b1b4a 14 DigitalOut M2(D7);
JonaVonk 0:6986a58c4515 15
JonaVonk 2:f832050b1b4a 16 PwmOut E1(D5);
JonaVonk 2:f832050b1b4a 17 PwmOut E2(D6);
JonaVonk 2:f832050b1b4a 18
JonaVonk 0:6986a58c4515 19
JonaVonk 0:6986a58c4515 20 MODSERIAL pc(USBTX, USBRX);
JonaVonk 0:6986a58c4515 21
JonaVonk 2:f832050b1b4a 22
JonaVonk 2:f832050b1b4a 23 void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes);
JonaVonk 0:6986a58c4515 24
JonaVonk 0:6986a58c4515 25 // main() runs in its own thread in the OS
JonaVonk 2:f832050b1b4a 26 int main()
JonaVonk 2:f832050b1b4a 27 {
JonaVonk 2:f832050b1b4a 28 float steps = 100;
JonaVonk 2:f832050b1b4a 29 pc.baud(115200);
JonaVonk 4:5f147787c2ca 30 while (true) {
JonaVonk 4:5f147787c2ca 31 for(int i = 0; i < steps; i++) {
JonaVonk 2:f832050b1b4a 32 moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
JonaVonk 2:f832050b1b4a 33 //pc.printf("step: %f\n\r", float(i)/steps);
JonaVonk 0:6986a58c4515 34 }
JonaVonk 4:5f147787c2ca 35 for(int i = steps; i > 0; i--) {
JonaVonk 2:f832050b1b4a 36 moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
JonaVonk 4:5f147787c2ca 37 }
JonaVonk 0:6986a58c4515 38 }
JonaVonk 0:6986a58c4515 39 }
JonaVonk 2:f832050b1b4a 40
JonaVonk 4:5f147787c2ca 41
JonaVonk 4:5f147787c2ca 42 //function to mave a motor to a certain number of rotations, counted from the start of the program.
JonaVonk 4:5f147787c2ca 43 //parameters:
JonaVonk 4:5f147787c2ca 44 //DigitalOut *M = pointer to direction cpntol pin of motor
JonaVonk 4:5f147787c2ca 45 //PwmOut *E = pointer to speed contorl pin of motor
JonaVonk 4:5f147787c2ca 46 //QEI *Enc = pointer to encoder of motor
JonaVonk 4:5f147787c2ca 47 //float rotDes = desired rotation
JonaVonk 2:f832050b1b4a 48 void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes)
JonaVonk 2:f832050b1b4a 49 {
JonaVonk 2:f832050b1b4a 50 float pErrorC;
JonaVonk 2:f832050b1b4a 51 float pErrorP = 0;
JonaVonk 2:f832050b1b4a 52 float iError = 0;
JonaVonk 2:f832050b1b4a 53 float dError;
JonaVonk 2:f832050b1b4a 54
JonaVonk 2:f832050b1b4a 55 float Kp = 5;
JonaVonk 2:f832050b1b4a 56 float Ki = 0.01;
JonaVonk 2:f832050b1b4a 57 float Kd = 0.7;
JonaVonk 2:f832050b1b4a 58
JonaVonk 2:f832050b1b4a 59 float rotC = Enc->getPulses()/(32*131.25);
JonaVonk 2:f832050b1b4a 60 float rotP = 0;
JonaVonk 2:f832050b1b4a 61 float MotorPWM;
JonaVonk 2:f832050b1b4a 62
JonaVonk 2:f832050b1b4a 63 Timer t;
JonaVonk 2:f832050b1b4a 64 float tieme = 0;
JonaVonk 2:f832050b1b4a 65
JonaVonk 2:f832050b1b4a 66 t.start();
JonaVonk 2:f832050b1b4a 67 do {
JonaVonk 2:f832050b1b4a 68 pErrorP = pErrorC;
JonaVonk 2:f832050b1b4a 69 pErrorC = rotDes - rotC;
JonaVonk 2:f832050b1b4a 70 iError = iError + pErrorC*tieme;
JonaVonk 2:f832050b1b4a 71 dError = (pErrorC - pErrorP)/tieme;
JonaVonk 2:f832050b1b4a 72
JonaVonk 2:f832050b1b4a 73 MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;
JonaVonk 2:f832050b1b4a 74
JonaVonk 2:f832050b1b4a 75 if(MotorPWM > 0) {
JonaVonk 2:f832050b1b4a 76 *M = 0;
JonaVonk 2:f832050b1b4a 77 *E = MotorPWM;
JonaVonk 2:f832050b1b4a 78 } else {
JonaVonk 2:f832050b1b4a 79 *M = 1;
JonaVonk 2:f832050b1b4a 80 *E = -MotorPWM;
JonaVonk 2:f832050b1b4a 81 }
JonaVonk 2:f832050b1b4a 82
JonaVonk 2:f832050b1b4a 83 rotP = rotC;
JonaVonk 2:f832050b1b4a 84 rotC = Enc->getPulses()/(32*131.25);
JonaVonk 2:f832050b1b4a 85 tieme = t.read();
JonaVonk 2:f832050b1b4a 86 t.reset();
JonaVonk 3:ecd394ae8118 87 } while (pErrorC > 0.001 || pErrorC < -0.001 ||dError > 0.01 || dError < -0.01);
JonaVonk 2:f832050b1b4a 88 t.stop();
JonaVonk 2:f832050b1b4a 89 }
JonaVonk 2:f832050b1b4a 90
JonaVonk 4:5f147787c2ca 91
JonaVonk 4:5f147787c2ca 92 //double that calculates the rotation of one arm.
JonaVonk 4:5f147787c2ca 93 //parameters:
JonaVonk 4:5f147787c2ca 94 //double xDes = ofset of the arm's shaft in cm in the x direction
JonaVonk 4:5f147787c2ca 95 //double yDes = ofset of the arm's shaft in cm in the y direction
JonaVonk 4:5f147787c2ca 96 double calcRot1(double xDes, double yDes)
JonaVonk 4:5f147787c2ca 97 {
JonaVonk 4:5f147787c2ca 98 return 6*((atan(yDes/xdes) - 0.5*(PI - acos((pow(xDes, 2) + pow(yDes, 2) - 2*pow(20, 2))/(-2*pow(20, 2)))))/(2*PI));
JonaVonk 4:5f147787c2ca 99 };
JonaVonk 4:5f147787c2ca 100
JonaVonk 4:5f147787c2ca 101 //double that calculates the rotation of the other arm.
JonaVonk 4:5f147787c2ca 102 //parameters:
JonaVonk 4:5f147787c2ca 103 //double xDes = ofset of the arm's shaft in cm in the x direction
JonaVonk 4:5f147787c2ca 104 //double yDes = ofset of the arm's shaft in cm in the y direction
JonaVonk 4:5f147787c2ca 105 double calcRot2(double xDes, double yDes)
JonaVonk 4:5f147787c2ca 106 {
JonaVonk 4:5f147787c2ca 107 return 6*((atan(yDes/xdes) + 0.5*(PI - acos((pow(xDes, 2) + pow(yDes, 2) - 2*pow(20, 2))/(-2*pow(20, 2)))))/(2*PI));
JonaVonk 4:5f147787c2ca 108 };
JonaVonk 4:5f147787c2ca 109