Numero Uno / Mbed 2 deprecated MotorDriver

code to drive the motors to the right position

Dependencies:   HIDScope QEI mbed

Fork of BMT-K9_potmeter_fade by First Last

main.cpp@6:d0f5da9962f5, 2015-09-25 (annotated)

Committer:
ewoud
Date:
Fri Sep 25 15:04:00 2015 +0000
Revision:
6:d0f5da9962f5
Parent:
5:edac3771ede4 
can now calculate the desired velocities from user input and position of the encoder

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ewoud 6:d0f5da9962f5 1 // main
ewoud 6:d0f5da9962f5 2 #include "mbed.h"
ewoud 6:d0f5da9962f5 3 #include <stdio.h>
ewoud 6:d0f5da9962f5 4 #include <math.h>
ewoud 6:d0f5da9962f5 5 #include "QEI.h"
ewoud 6:d0f5da9962f5 6 #include "Point.cpp"
ewoud 6:d0f5da9962f5 7
ewoud 6:d0f5da9962f5 8 Serial pc(USBTX, USBRX);
ewoud 6:d0f5da9962f5 9 AnalogIn pot1(A0); // sets requested Vx
ewoud 6:d0f5da9962f5 10 AnalogIn pot2(A1); // sets requested Vy
ewoud 6:d0f5da9962f5 11 QEI encMotor1 (D12, D13, NC, 624);
ewoud 6:d0f5da9962f5 12 QEI encMotor2 (D10, D11, NC, 624);
ewoud 6:d0f5da9962f5 13 float round=4200;
ewoud 6:d0f5da9962f5 14 float angle1;
ewoud 6:d0f5da9962f5 15 float angle2;
ewoud 6:d0f5da9962f5 16 Point pCurrent;
ewoud 6:d0f5da9962f5 17 Point pTo;
ewoud 6:d0f5da9962f5 18 float toX;
ewoud 6:d0f5da9962f5 19 float toY;
ewoud 6:d0f5da9962f5 20 float deltaAngle1;
ewoud 6:d0f5da9962f5 21 float deltaAngle2;
ewoud 6:d0f5da9962f5 22 float v1;
ewoud 6:d0f5da9962f5 23 float v2;
ewoud 6:d0f5da9962f5 24 float timestep=1;
ewoud 6:d0f5da9962f5 25 int main()
ewoud 6:d0f5da9962f5 26 {
ewoud 6:d0f5da9962f5 27 while(true){
ewoud 6:d0f5da9962f5 28 // first get the current position from the motor encoders
ewoud 6:d0f5da9962f5 29 angle1=encMotor1.getPulses()/round*360;
ewoud 6:d0f5da9962f5 30 angle2=encMotor2.getPulses()/round*360;
ewoud 6:d0f5da9962f5 31 pCurrent.fromRotational(angle1,angle2);
ewoud 6:d0f5da9962f5 32
ewoud 6:d0f5da9962f5 33
ewoud 6:d0f5da9962f5 34 // calculate the position to go to according the the current position + the distance that should be covered in this timestep
ewoud 6:d0f5da9962f5 35 toX=pCurrent.posX+pot1.read()*timestep;
ewoud 6:d0f5da9962f5 36 toY=pCurrent.posY+pot2.read()*timestep;
ewoud 6:d0f5da9962f5 37
ewoud 6:d0f5da9962f5 38 pTo.fromCarthesian(toX, toY);
ewoud 6:d0f5da9962f5 39
ewoud 6:d0f5da9962f5 40 // calculate how much the angles should change in this timestep
ewoud 6:d0f5da9962f5 41 deltaAngle1=pTo.rotA-pCurrent.rotA;
ewoud 6:d0f5da9962f5 42 deltaAngle2=pTo.rotB-pCurrent.rotB;
ewoud 6:d0f5da9962f5 43
ewoud 6:d0f5da9962f5 44 // calculate the neccesairy velocities to make these angles happen.
ewoud 6:d0f5da9962f5 45 v1=deltaAngle1/timestep;
ewoud 6:d0f5da9962f5 46 v2=deltaAngle2/timestep;
ewoud 6:d0f5da9962f5 47
ewoud 6:d0f5da9962f5 48 // output to the user
ewoud 6:d0f5da9962f5 49 pc.printf("now: angle1: %f, angle2: %f, x: %f, y: %f \n\r",pCurrent.rotA,pCurrent.rotB,pCurrent.posX,pCurrent.posY);
ewoud 6:d0f5da9962f5 50 pc.printf("to: angle1: %f, angle2: %f, x: %f, y: %f \n\r",pTo.rotA,pTo.rotB,pTo.posX,pTo.posY);
ewoud 6:d0f5da9962f5 51 pc.printf("v1: %f, v2: %f \n\r",v1,v2);
ewoud 6:d0f5da9962f5 52 wait(timestep);
ewoud 6:d0f5da9962f5 53 }
ewoud 6:d0f5da9962f5 54 }