Jona Vonk
/
MoveMotors
Hier zitten extra leipe functies in naast PID ook x en y naar hoeken
Revision 4:5f147787c2ca, committed 2019-10-24
- Comitter:
- JonaVonk
- Date:
- Thu Oct 24 11:46:00 2019 +0000
- Parent:
- 3:ecd394ae8118
- Commit message:
- Hier zitten mega advanced functies bij
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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)); +}; +