main.cpp

00001 /* mbed Microcontroller Library
00002  * Copyright (c) 2018 ARM Limited
00003  * SPDX-License-Identifier: Apache-2.0
00004  */
00005 
00006 #include "mbed.h"
00007 #include "MODSERIAL.h"
00008 #include "QEI.h"
00009 #include "math.h"
00010 
00011 QEI Encoder(D12,D13,NC,32);
00012 
00013 DigitalOut M1(D4);
00014 DigitalOut M2(D7);
00015 
00016 PwmOut E1(D5);
00017 PwmOut E2(D6);
00018 
00019 
00020 MODSERIAL pc(USBTX, USBRX);
00021 
00022 
00023 void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes);
00024 
00025 // main() runs in its own thread in the OS
00026 int main()
00027 {
00028     float steps = 100;
00029     pc.baud(115200);
00030     while (true) {
00031         for(int i = 0; i < steps; i++) {
00032             moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
00033             //pc.printf("step: %f\n\r", float(i)/steps);
00034         }
00035         for(int i = steps; i > 0; i--) {
00036             moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
00037         }
00038     }
00039 }
00040 
00041 
00042 //function to mave a motor to a certain number of rotations, counted from the start of the program.
00043 //parameters:
00044 //DigitalOut *M = pointer to direction cpntol pin of motor
00045 //PwmOut *E = pointer to speed contorl pin of motor
00046 //QEI *Enc = pointer to encoder of motor
00047 //float rotDes = desired rotation
00048 void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes)
00049 {
00050     float pErrorC;
00051     float pErrorP = 0;
00052     float iError = 0;
00053     float dError;
00054 
00055     float Kp = 5;
00056     float Ki = 0.01;
00057     float Kd = 0.7;
00058 
00059     float rotC = Enc->getPulses()/(32*131.25);
00060     float rotP = 0;
00061     float MotorPWM;
00062 
00063     Timer t;
00064     float tieme = 0;
00065 
00066     t.start();
00067     do {
00068         pErrorP = pErrorC;
00069         pErrorC = rotDes - rotC;
00070         iError = iError + pErrorC*tieme;
00071         dError = (pErrorC - pErrorP)/tieme;
00072 
00073         MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;
00074 
00075         if(MotorPWM > 0) {
00076             *M = 0;
00077             *E = MotorPWM;
00078         } else {
00079             *M = 1;
00080             *E = -MotorPWM;
00081         }
00082 
00083         rotP = rotC;
00084         rotC = Enc->getPulses()/(32*131.25);
00085         tieme = t.read();
00086         t.reset();
00087     } while (pErrorC > 0.001 || pErrorC < -0.001 ||dError > 0.01 || dError < -0.01);
00088     t.stop();
00089 }
00090 
00091 
00092 //double that calculates the rotation of one arm.
00093 //parameters:
00094 //double xDes = ofset of the arm's shaft in cm in the x direction
00095 //double yDes = ofset of the arm's shaft in cm in the y direction
00096 double calcRot1(double xDes, double yDes)
00097 {
00098     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));
00099 };
00100 
00101 //double that calculates the rotation of the other arm.
00102 //parameters:
00103 //double xDes = ofset of the arm's shaft in cm in the x direction
00104 //double yDes = ofset of the arm's shaft in cm in the y direction
00105 double calcRot2(double xDes, double yDes)
00106 {
00107     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));
00108 };
00109