main.cpp

00001 // main
00002 #include "mbed.h"
00003 #include <stdio.h>
00004 #include <math.h>
00005 #include "QEI.h"
00006 #include "Point.cpp"
00007 
00008 Serial pc(USBTX, USBRX);
00009 AnalogIn pot1(A0); // sets requested Vx
00010 AnalogIn pot2(A1); // sets requested Vy
00011 QEI encMotor1 (D12, D13, NC, 624);
00012 QEI encMotor2 (D10, D11, NC, 624);
00013 float round=4200;
00014 float angle1;
00015 float angle2;
00016 Point pCurrent;
00017 Point pTo;
00018 float toX;
00019 float toY;
00020 float deltaAngle1;
00021 float deltaAngle2;
00022 float v1;
00023 float v2;
00024 float timestep=1;
00025 int main()
00026 {
00027     while(true){
00028         // first get the current position from the motor encoders
00029         angle1=encMotor1.getPulses()/round*360;
00030         angle2=encMotor2.getPulses()/round*360;
00031         pCurrent.fromRotational(angle1,angle2);
00032         
00033         
00034         // calculate the position to go to according the the current position + the distance that should be covered in this timestep
00035         toX=pCurrent.posX+pot1.read()*timestep;
00036         toY=pCurrent.posY+pot2.read()*timestep;
00037         
00038         pTo.fromCarthesian(toX, toY);
00039         
00040         // calculate how much the angles should change in this timestep
00041         deltaAngle1=pTo.rotA-pCurrent.rotA;
00042         deltaAngle2=pTo.rotB-pCurrent.rotB;
00043         
00044         // calculate the neccesairy velocities to make these angles happen.
00045         v1=deltaAngle1/timestep;
00046         v2=deltaAngle2/timestep;
00047         
00048         // output to the user
00049         pc.printf("now: angle1: %f, angle2: %f, x: %f, y: %f \n\r",pCurrent.rotA,pCurrent.rotB,pCurrent.posX,pCurrent.posY);
00050         pc.printf("to:  angle1: %f, angle2: %f, x: %f, y: %f \n\r",pTo.rotA,pTo.rotB,pTo.posX,pTo.posY);
00051         pc.printf("v1: %f, v2: %f \n\r",v1,v2);
00052         wait(timestep);
00053     }
00054 }