Diff: main.cpp

Revision:

4:5f147787c2ca

Parent:

3:ecd394ae8118

--- a/main.cpp	Mon Oct 21 12:32:20 2019 +0000
+++ b/main.cpp	Thu Oct 24 11:46:00 2019 +0000
@@ -6,6 +6,7 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "QEI.h"
+#include "math.h"
 
 QEI Encoder(D12,D13,NC,32);
 
@@ -15,37 +16,35 @@
 PwmOut E1(D5);
 PwmOut E2(D6);
 
-AnalogIn Pot1(A0);
-AnalogIn Pot2(A1);
-
-float potVal1;
-float potVal2;
-
-float pi = 3.14159265359;
-
 
 MODSERIAL pc(USBTX, USBRX);
 
 
 void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes);
 
-
 // main() runs in its own thread in the OS
 int main()
 {
     float steps = 100;
     pc.baud(115200);
-    while (true){
-        for(int i = 0; i < steps; i++){
+    while (true) {
+        for(int i = 0; i < steps; i++) {
             moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
             //pc.printf("step: %f\n\r", float(i)/steps);
         }
-        for(int i = steps; i > 0; i--){
+        for(int i = steps; i > 0; i--) {
             moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
-        }            
+        }
     }
 }
 
+
+//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 rotDes)
 {
     float pErrorC;
@@ -85,9 +84,26 @@
         rotC = Enc->getPulses()/(32*131.25);
         tieme = t.read();
         t.reset();
-        //pc.printf("pError: %f dError: %f iError: %f PWM: %f\n\r", pErrorC, dError, iError, MotorPWM);
     } while (pErrorC > 0.001 || pErrorC < -0.001 ||dError > 0.01 || dError < -0.01);
-    //*E = 0;
     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) + pow(yDes, 2) - 2*pow(20, 2))/(-2*pow(20, 2)))))/(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) + pow(yDes, 2) - 2*pow(20, 2))/(-2*pow(20, 2)))))/(2*PI));
+};
+