Revision 6:d0f5da9962f5, committed 2015-09-25

Comitter:

ewoud

Date:

Fri Sep 25 15:04:00 2015 +0000

Parent:

5:edac3771ede4

Commit message:

can now calculate the desired velocities from user input and position of the encoder

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Point.cpp	Fri Sep 25 15:04:00 2015 +0000
@@ -0,0 +1,57 @@
+// Example: data structs and functions
+#include <stdio.h>
+#include <math.h>
+
+const double M_PI =3.141592653589793238463;
+const float l = 10; // distance between the motors
+const float armlength=15; // length of the arms from the motor
+    
+class Point
+{
+    public:
+    
+    float posX;
+    float posY;
+    float rotA;
+    float rotB;
+
+    bool fromCarthesian(float x, float y)
+    {
+        posX = x;
+        posY = y;
+        rotA = atan(y/x)*180/M_PI;
+        rotB = atan(y/(l-x))*180/M_PI;
+        
+        // function is done, return the struct type Point:
+        return true;
+    }
+    bool fromRotational(float a, float b)
+    {
+        rotA = a*M_PI/180;
+        rotB = b*M_PI/180;
+        posX = (tan(rotB)*l)/(tan(rotA)+tan(rotB));
+        posY = tan(rotA)*posX;
+        
+        return true;
+        
+    }
+    bool checkbounds()
+    {
+        if (rotA <= 0 or rotB <= 0){
+            return 0;
+        }
+        if (rotA > 90 or rotB > 90){
+            return 0;
+        }
+        if (sqrt(pow(posX,2)+pow(posY,2)) > armlength){ // too far from left arm
+            return 0;
+        }
+        if (sqrt(pow(l-posX,2)+pow(posY,2)) > armlength){ // too far from right arm
+            return 0;
+        }
+        return true;
+    }
+};
+
+
+

--- 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);
+    }
+}