Diff: main.cpp

Revision:

6:d0f5da9962f5

Parent:

5:edac3771ede4

--- a/main.cpp	Thu Sep 24 09:51:21 2015 +0000
+++ b/main.cpp	Fri Sep 25 15:04:00 2015 +0000
@@ -1,64 +1,54 @@
-#include "mbed.h"
-#include "HIDScope.h"
-#include "QEI.h"
-// test if colaboration is working
-// myled is an object of class PwmOut. It uses the LED_RED pin
-// in human speech: myled is an output that can be controlled with PWM. LED_RED is the pin which is connected to the output
-PwmOut myled2(D5);
-PwmOut myled1(D6);
-
-DigitalOut motor1direction(D7);
-// pot is an object of class AnalogIn. It uses the PTB0 pin
-// in human speech: pot is an analog input. You can read the voltage on pin PTB0
-AnalogIn pot1(A0);
-AnalogIn pot2(A1);
-
-//HIDScope scope(1);
-Serial pc(USBTX, USBRX);
-QEI wheel (D12, D13, NC, 624);
-float lastpotread = 0;
-int countsPerRound = 32*131;
-float gototick;
-int currentpulses;
-int errorsignal;
-float Kf=0.2;
-//start 'main' function. Should be done once in every C(++) program
-int main()
-{
-    //setup some stuff
-    //period of PWM signal is 10kHz. Every 100 microsecond a new PWM period is started
-    myled1.period_ms(0.1);
-    myled2.period_ms(0.1);
-    myled1=0.5;
-    //motor1=1;
-    //while 1 is unequal to zero. For humans: loop forever
-    while(1) {
-        currentpulses=wheel.getPulses();
-        gototick = pot1.read()*countsPerRound;
-        errorsignal=gototick-currentpulses;
-        if (lastpotread != pot1.read()){
-            lastpotread=pot1.read();
-            pc.printf("potvalue: %f, position: %d, control speed: %f \n\r",gototick,currentpulses,errorsignal*Kf);
-        }
-        
-        if (errorsignal > 0)
-        {
-            motor1direction=0; 
-            myled1=errorsignal*Kf;
-            
-        }
-        else
-        {
-            motor1direction=1;
-            myled1=-errorsignal*Kf;
-        }
-        
-        
-        //myled1.write(pot1.read());
-        //myled2.write(pot2.read());
-        //wait some time to give the LED output a few PWM cycles. Otherwise a new value is written before the previously set PWM period (of 100microseconds) is finished
-        //This loop executes at roughly 100Hz (1/0.01s)
-        wait(0.01);
-    }
-}
-
+// main
+#include "mbed.h"
+#include <stdio.h>
+#include <math.h>
+#include "QEI.h"
+#include "Point.cpp"
+
+Serial pc(USBTX, USBRX);
+AnalogIn pot1(A0); // sets requested Vx
+AnalogIn pot2(A1); // sets requested Vy
+QEI encMotor1 (D12, D13, NC, 624);
+QEI encMotor2 (D10, D11, NC, 624);
+float round=4200;
+float angle1;
+float angle2;
+Point pCurrent;
+Point pTo;
+float toX;
+float toY;
+float deltaAngle1;
+float deltaAngle2;
+float v1;
+float v2;
+float timestep=1;
+int main()
+{
+    while(true){
+        // first get the current position from the motor encoders
+        angle1=encMotor1.getPulses()/round*360;
+        angle2=encMotor2.getPulses()/round*360;
+        pCurrent.fromRotational(angle1,angle2);
+        
+        
+        // calculate the position to go to according the the current position + the distance that should be covered in this timestep
+        toX=pCurrent.posX+pot1.read()*timestep;
+        toY=pCurrent.posY+pot2.read()*timestep;
+        
+        pTo.fromCarthesian(toX, toY);
+        
+        // calculate how much the angles should change in this timestep
+        deltaAngle1=pTo.rotA-pCurrent.rotA;
+        deltaAngle2=pTo.rotB-pCurrent.rotB;
+        
+        // calculate the neccesairy velocities to make these angles happen.
+        v1=deltaAngle1/timestep;
+        v2=deltaAngle2/timestep;
+        
+        // output to the user
+        pc.printf("now: angle1: %f, angle2: %f, x: %f, y: %f \n\r",pCurrent.rotA,pCurrent.rotB,pCurrent.posX,pCurrent.posY);
+        pc.printf("to:  angle1: %f, angle2: %f, x: %f, y: %f \n\r",pTo.rotA,pTo.rotB,pTo.posX,pTo.posY);
+        pc.printf("v1: %f, v2: %f \n\r",v1,v2);
+        wait(timestep);
+    }
+}