Revision 5:8ab3fcb8f4f8, committed 2019-10-29

Comitter:

JonaVonk

Date:

Tue Oct 29 11:27:56 2019 +0000

Parent:

4:55e6707949dd

Commit message:

Jona's kleine code;

Changed in this revision

--- a/main.cpp	Fri Oct 25 10:35:11 2019 +0000
+++ b/main.cpp	Tue Oct 29 11:27:56 2019 +0000
@@ -1,80 +1,94 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2018 ARM Limited
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
 #include "mbed.h"
-//#include "HIDScope.h"
+#include "MODSERIAL.h"
 #include "QEI.h"
-#include "MODSERIAL.h"
-//#include "BiQuad.h"
-//#include "FastPWM.h"
 #include <vector>
 
 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);
+DigitalOut M1(D4);  //Direction control
+DigitalOut M2(D7);  //Direction control
 
-PwmOut E1(D5);
-PwmOut E2(D6);
-
-double initRot1 = 0;
-double initRot2 = 48.5/360;
+PwmOut E1(D5);      //Speed control
+PwmOut E2(D6);      //Speed control
 
 
 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("Start\n\r");
-    moveAlongPath(30, 0, 0, 20.0);
-    pc.printf("End");
-    
-}
+void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes);
 
 
-//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)
+int main()
 {
-    float pErrorC;
-    float pErrorP = 0;
-    float iError = 0;
-    float dError;
+    //double steps = 100;
+    vector<double> mot1;
+    vector<double> mot2;
+
+    mot1.push_back(0.1);
+    mot1.push_back(0.2);
+    mot1.push_back(0.3);
+    mot1.push_back(0.4);
+    mot1.push_back(0.3);
+    mot1.push_back(0.2);
+    mot1.push_back(0.1);
+    mot1.push_back(0.0);
 
-    float Kp = 5;
-    float Ki = 0.01;
-    float Kd = 0.7;
+    mot2.push_back(0.1);
+    mot2.push_back(0.2);
+    mot2.push_back(0.3);
+    mot2.push_back(0.4);
+    mot2.push_back(0.3);
+    mot2.push_back(0.2);
+    mot2.push_back(0.1);
+    mot2.push_back(0.0);
+    pc.baud(115200);
+    while (true) {
+        for(int i = 0; i < 8; i++){
+            moveMotorTo(&M1, &E1, &Enc1, 0, 1, mot1.at(i));
+            pc.printf("mot1\n\r");
+            moveMotorTo(&M2, &E2, &Enc2, 0, -1, mot2.at(i));
+            pc.printf("mot2\n\r");
+        }
+    }
+}
 
-    float rotC = Enc->getPulses()/(32*131.25) + initRot;
-    float rotP = 0;
-    float MotorPWM;
+void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
+{
+    double pErrorC;
+    double pErrorP = 0;
+    double iError = 0;
+    double dError;
+
+    double Kp = 300;
+    double Ki = 0;
+    double Kd = 10;
+
+    double rotC = Enc->getPulses()/(32*131.25) + initRot;
+    double MotorPWM;
+    //pc.printf("\n\n");
 
     Timer t;
-    float tieme = 0;
+    double tieme = 0;
 
     t.start();
     do {
         pErrorP = pErrorC;
+        rotC = Enc->getPulses()/(32*131.25) + initRot;
         pErrorC = rotDes - rotC;
+        tieme = t.read();
+        t.reset();
         iError = iError + pErrorC*tieme;
         dError = (pErrorC - pErrorP)/tieme;
 
-        MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;
+        MotorPWM = (pErrorC*Kp + iError*Ki + dError*Kd)*dir;
 
         if(MotorPWM > 0) {
             *M = 0;
@@ -83,72 +97,9 @@
             *M = 1;
             *E = -MotorPWM;
         }
-
-        rotP = rotC;
-        rotC = Enc->getPulses()/(32*131.25) + initRot;
-        tieme = t.read();
-        t.reset();
-        pc.printf("pError: %f iError: %f dError: %f\n\r", pErrorC, iError, dError);
-    } while (pErrorC > 0.001 || pErrorC < -0.001 ||dError > 0.01 || dError < -0.01);
+        wait(0.01);
+        //pc.printf("pError: %f iError: %f dError: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
+    } while (abs(MotorPWM) > 0.029 || abs(dError) > 0.001);
+    //pc.printf("pError: %f iError: %f dError: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
     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)
-{
-    pc.printf("rot2: %f", 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)));
-    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)
-{
-    pc.printf("xS: %f xE: %f yS: %f yE: %f\n\r", xStart, xEnd, yStart,yEnd);
-    double lPath = sqrt(pow(xEnd-xStart, 2.0) + pow(yEnd-yStart, 2.0));
-    int noSteps = int(lPath/0.1);
-    double xStep = (xEnd - xStart)/double(noSteps);
-    double yStep = (yEnd - yStart)/double(noSteps);
-    pc.printf("xStep: %f yStep: %f\n\r", xStep, yStep);
-    pc.printf("dx: %f dy: %f\n\r", xEnd - xStart, yEnd - yStart);
-    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)
-{
-    pc.printf("xS: %f xE: %f yS: %f yE: %f \n\r", xStart, xEnd, yStart,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());
-    }
-}
-
+}
\ No newline at end of file

--- a/mbed.bld	Fri Oct 25 10:35:11 2019 +0000
+++ b/mbed.bld	Tue Oct 29 11:27:56 2019 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/mbed_official/code/mbed/builds/d1b4690b3f8b
\ No newline at end of file
+https://os.mbed.com/users/mbed_official/code/mbed/builds/675da3299148
\ No newline at end of file