Diff: main.cpp

Revision:

2:96e093a48314

Parent:

1:b862262a9d14

diff -r b862262a9d14 -r 96e093a48314 main.cpp
--- a/main.cpp	Wed Sep 04 15:30:13 2019 +0000
+++ b/main.cpp	Fri Oct 25 09:37:26 2019 +0000
@@ -1,23 +1,148 @@
 #include "mbed.h"
 //#include "HIDScope.h"
-//#include "QEI.h"
+#include "QEI.h"
 #include "MODSERIAL.h"
 //#include "BiQuad.h"
 //#include "FastPWM.h"
+#include <vector>
 
-DigitalOut led(LED_RED);
+using std::vector;
+
+double Pi = 3.14159265359;
+
+QEI Enc1(D12, D13, NC, 32);
+QEI Enc2(D10, D11, NC, 32);
+
+DigitalOut M1(D4);
+DigitalOut M2(D7);
+
+PwmOut E1(D5);
+PwmOut E2(D6);
+
+double initRot1 = 0;
+double initRot2 = 48.5/360;
+
 
 MODSERIAL pc(USBTX, USBRX);
 
+
+void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes);
+double calcRot1(double xDes, double yDes);
+double calcRot2(double xDes, double yDes);
+void plotPath(double xStart, double yStart, double xEnd, double yEnd, double *xPath[], double *yPath[]);
+void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd);
+
+// main() runs in its own thread in the OS
 int main()
 {
     pc.baud(115200);
-    pc.printf("\r\nStarting...\r\n\r\n");
+    pc.printf("Start\n\r");
+    moveAlongPath(0, 0, 0, 20);
+    pc.printf("End");
     
-    while (true) {
-        
-        led = !led;
-        
-        wait_ms(500);
+}
+
+
+//function to mave a motor to a certain number of rotations, counted from the start of the program.
+//parameters:
+//DigitalOut *M = pointer to direction cpntol pin of motor
+//PwmOut *E = pointer to speed contorl pin of motor
+//QEI *Enc = pointer to encoder of motor
+//float rotDes = desired rotation
+void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float initRot, float rotDes)
+{
+    float pErrorC;
+    float pErrorP = 0;
+    float iError = 0;
+    float dError;
+
+    float Kp = 5;
+    float Ki = 0.01;
+    float Kd = 0.7;
+
+    float rotC = Enc->getPulses()/(32*131.25) + initRot;
+    float rotP = 0;
+    float MotorPWM;
+
+    Timer t;
+    float tieme = 0;
+
+    t.start();
+    do {
+        pErrorP = pErrorC;
+        pErrorC = rotDes - rotC;
+        iError = iError + pErrorC*tieme;
+        dError = (pErrorC - pErrorP)/tieme;
+
+        MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;
+
+        if(MotorPWM > 0) {
+            *M = 0;
+            *E = MotorPWM;
+        } else {
+            *M = 1;
+            *E = -MotorPWM;
+        }
+
+        rotP = rotC;
+        rotC = Enc->getPulses()/(32*131.25) + initRot;
+        tieme = t.read();
+        t.reset();
+    } while (pErrorC > 0.001 || pErrorC < -0.001 ||dError > 0.01 || dError < -0.01);
+    t.stop();
+}
+
+
+//double that calculates the rotation of one arm.
+//parameters:
+//double xDes = ofset of the arm's shaft in cm in the x direction
+//double yDes = ofset of the arm's shaft in cm in the y direction
+double calcRot1(double xDes, double yDes)
+{
+    return 6*((atan(yDes/xDes) - 0.5*(Pi - acos((pow(xDes, 2.0) + pow(yDes, 2.0) - 2*pow(20.0, 2.0))/(-2*pow(20.0, 2.0)))))/(2*Pi));
+};
+
+//double that calculates the rotation of the other arm.
+//parameters:
+//double xDes = ofset of the arm's shaft in cm in the x direction
+//double yDes = ofset of the arm's shaft in cm in the y direction
+double calcRot2(double xDes, double yDes)
+{
+    return 6*((atan(yDes/xDes) + 0.5*(Pi - acos((pow(xDes, 2.0) + pow(yDes, 2.0) - 2*pow(20.0, 2.0))/(-2*pow(20.0, 2.0)))))/(2*Pi));
+};
+
+void plotPath(double xStart, double yStart, double xEnd, double yEnd, vector<double> *xPath, vector<double> *yPath)
+{
+    double lPath = sqrt(pow(xEnd-xStart, 2.0) + pow(yEnd-yStart, 2.0));
+    int noSteps = int(lPath/0.1);
+    double xStep = (xEnd - xStart)/noSteps;
+    double yStep = (yEnd - yStart)/noSteps;
+    for(int i = 0; i<=noSteps; i++) {
+        xPath->push_back(xStart + i*xStep);
+        yPath->push_back(yStart + i*yStep);
+        pc.printf("to go plotPath: %i / %i \n\r", i, noSteps);
     }
 }
+
+void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd)
+{
+    vector<double> xPath;
+    vector<double> yPath;
+    vector<double> rot1Path;
+    vector<double> rot2Path;
+    
+    plotPath(xStart, yStart, xEnd, yEnd, &xPath, &yPath);
+
+    for(int i = 0; i < xPath.size(); i++) {
+        rot1Path.push_back(calcRot1(xPath[i], yPath[i]));
+        rot2Path.push_back(calcRot2(xPath[i], yPath[i]));
+        pc.printf("to go angle: %i / %i \n\r", i, xPath.size());
+    }
+
+    for(int i = 0; i < xPath.size(); i++) {
+        moveMotorTo(&M1, &E1, &Enc1, initRot1, rot1Path.at(i));
+        moveMotorTo(&M2, &E2, &Enc2, initRot2, rot2Path.at(i));
+        pc.printf("to go move: %i / %i", i, rot1Path.size());
+    }
+}
+