Diff: main.cpp

Revision:

10:a3dd8ec4be60

Parent:

9:4db47b8eb750

Child:

11:8a0fecc86705

--- a/main.cpp	Wed Mar 06 06:59:06 2013 +0000
+++ b/main.cpp	Mon Mar 25 15:54:31 2013 +0000
@@ -1,34 +1,26 @@
-/*
-~~~~~~present status~~~~~~
-    * X Y data from sereal.  vx and vy are culculated like -> (new X,Y) - (previous X,Y) / timer
-        --  i don't know how to send data quickly
-    
-    now I'm fighting to other problems...
-*/
-
-
 #include "mbed.h"
 #include "laserProjectorHardware.h"
 #include <vector>
 #define PI 3.141592
 
-Ticker changeXY;
+InterruptIn clockEncoderPin(p5);
+DigitalIn directionPin(p6);
 
 void processSerial();
-Timer timer;
+Timer timer_v;
 LocalFileSystem local("local");
 //Serial pc(USBTX, USBRX);
 
-unsigned int X, Y, T;   // these datas are from other tracking system... 
+unsigned int X, Y, T;
 unsigned int beforeX , beforeY;
 int startX = CENTER_AD_MIRROR_X;
 int startY = CENTER_AD_MIRROR_Y;
 
 float sint=0, cost=1;
           
-float vx = 0, vy=0;//omegaX=0, omegaY=0; 
-float theta=0, dt_betwords=200, st=400, radious=1500;
-unsigned int dt=500, timecounter = 0, ticktime=600;
+float vx = 0, vy=0;
+float theta=0, dt_betwords=200, st=400;
+unsigned int dt=500, ticktime=600;
 
 bool newPositionReady=false;
 unsigned int counter=0;
@@ -45,51 +37,34 @@
     vector<point2d> letpoints;
 };
 
-//int points[200][2] = {};
 vector<letter> libletter;   // letter library
 
-vector<int> buf;                            //buffer vector
-vector<int> buf2;
+////for culculate rotary velocity --------------------------------------------------------
 
-/*void makeBuffer()
-{
-    float sint = sin(theta);
-    float cost = cos(theta);
-
-    startX = CENTER_AD_MIRROR_X;
-    startY = CENTER_AD_MIRROR_Y;
+volatile float angleIncrement = 2.0*PI / 128; // when in Sign/Magnitude mode
+float radious = 100;
 
-    buf.clear();
-/*conversion points to buf before writing;
-    for(int i=0; i<inputletters.size(); i++) {
-        
-        buf.push_back(222);
-        buf.push_back(222);
-        
-        for(int j=0; j<libletter[inputletters[i]-'a'].letpoints.size(); j++) {
-            int x = 0.8*dt_betwords/1000*(libletter[inputletters[i]-'a'].letpoints[j].x +500*(i-inputletters.size()/2.0));
-            int y = -dt_betwords/1000*(libletter[inputletters[i]-'a'].letpoints[j].y);
-            buf.push_back(x);
-            buf.push_back(y);
-        }
-    }
+// a ticker function to compute the speed periodically:
+#define PERIODIC_COMPUTE 10000 // in us
+volatile float angularSpeed = 0;
+volatile float angle=0, oldAngle=0;
+Ticker speedTimerCompute;
 
-}
-*/
-
-void CircleXY(){
-    X = CENTER_AD_MIRROR_X + radious * cos(2*PI*(float)timecounter/(float)ticktime);
-    Y = CENTER_AD_MIRROR_X + radious * sin(2*PI*(float)timecounter/(float)ticktime);
-    vx = radious * (float)PI * (-sin(2*PI*(float)timecounter/(float)ticktime));
-    vy = radious * (float)PI * cos(2*PI*(float)timecounter/(float)ticktime);
-    theta = atan2(vy,vx);
-    cost = -cos(theta);
-    sint = -sin(theta);
-    timecounter ++;
+// the external interrupt routine:
+void encoderClock() {
+    if (directionPin) angle += angleIncrement ; else angle -= angleIncrement;
 }
 
+void computeSpeed() {
+    // We know exactly how much time passed since we last computed the speed, this is PERIODIC_COMPUTE in microseconds
+    angularSpeed = 1000000.0 * (angle-oldAngle) / (float)(PERIODIC_COMPUTE); // in rad/sec
+    oldAngle=angle;
+}
+   
+Timer timer;
+
 int main(){
-
+    //read from TextFileLibrary ------------------------------------------------------
     FILE *fp = fopen("/local/text.txt", "r");
     if(!fp) {
         IO.setGreenPower(1);
@@ -112,11 +87,19 @@
     }
 
 
-    //makeBuffer();
-
     // SETUP: --------------------------------------------------------------------------------------------
     IO.init(); // note: serial speed can be changed by checking in the hardwareIO.cpp initialization
 
+    // initialize the angle (arbitrary origin):
+    oldAngle=angle=0;
+  
+    // Attach the external interrupt routine:
+    clockEncoderPin.rise(&encoderClock);
+    timer.reset(); timer.start();
+    
+    // Attach the periodic computing function:
+    speedTimerCompute.attach_us(&computeSpeed, PERIODIC_COMPUTE); 
+ 
     // Set displaying laser powers:
     IO.setRedPower(0);
     IO.setGreenPower(0);
@@ -124,25 +107,38 @@
 
     X = beforeX = CENTER_AD_MIRROR_X;
     Y = beforeY = CENTER_AD_MIRROR_Y;
-    timer.start();
-    //changeXY.attach_us(&CircleXY, 200);
+    timer_v.start();
     // MAIN LOOP: --------------------------------------------------------------------------------------------
     while(1) {
     if (pc.readable()>0) processSerial();
     
     if(start) {
-        timer.reset();
+        
+        timer_v.reset();
         wait_us(5000);
+        //culculating velocity -----------------------------------
+           //velocity valuable ::  sint, cost, theta, vx, vy
+        
+        // send the speed on the serial port every 30 ms:
+        if (timer.read_ms()>1000) {
+            pc.printf("Angular Speed = %4.2f\t Cumulative Angle = %4.2f\n" , angularSpeed, angle);
+            sint = sin(angle);
+            cost = cos(angle);
+            timer.reset();
+        }   
+        
+        
+        // drawing ///-----------------------------------------------
         for(int i=0; i<inputletters.size(); i++) {
         
             for(int j=0; j<libletter[inputletters[i]-'a'].letpoints.size(); j++) {
             
                 if (pc.readable()>0) processSerial();
                 
-                int x = -0.8*((libletter[inputletters[i]-'a'].letpoints[j].x ) - sqrtf(vx*vx+vy*vy)*timer.read_us()/st*j/100); //this ZURE should be modifyed not by dt but Timer.
+                int x = -0.8*((libletter[inputletters[i]-'a'].letpoints[j].x ) - radious*angle*timer_v.read_us()/st*j/100); //this ZURE should be modifyed not by dt but Timer.
                 int y = (libletter[inputletters[i]-'a'].letpoints[j].y);
                 IO.writeOutXY(X+cost*x - sint*y,Y + sint*x + cost*y);
-                timer.reset();
+                timer_v.reset();
                 wait_us(dt);
                 IO.setRedPower(libletter[inputletters[i]-'a'].letpoints[j].laserSwitch);//on
             }
@@ -150,38 +146,14 @@
             wait_us(dt_betwords);//sqrtf(vx*vx+vy*vy)*100000);
 
         }
-        
+        ////////////////
         IO.setRGBPower(1); //off
         wait_us(10000);
         start=false;
         
         }
         
-        /*for(int j=0; j<buf.size()/2; j++) {
-            if ( (buf[j*2] == 222) && (buf[j*2+1] == 222)){
-                IO.setRGBPower(1);
-                int xx=buf[j*2+2]- vx*dt/1000*j;
-                int yy=buf[j*2+3];//- vy*dt/1000*j;
-                IO.writeOutXY(startX+cost*xx + sint*yy,startY-sint*xx + cost*yy);
-                wait(0.0004);
-                IO.setRGBPower(0);
-            }
-            else{
-            timer.start();
-            int xx=buf[j*2]- vx*dt/1000*j;
-            int yy=buf[j*2+1];//- vy*dt/1000*j;
-            IO.writeOutXY(startX+cost*xx + sint*yy,startY-sint*xx + cost*yy);
-            wait_us(dt);
-            IO.setRGBPower(0); //on
-            timer.stop();
-            //dt=timer.read();
-            }
-            
-            timecounter += dt;
-            if(timecounter >= T) timecounter = 0; 
-        }*/
-        //buf2.clear();
-        //wait(0.02);
+        
 
     }
 }
@@ -219,7 +191,7 @@
             stringData[indexStringData] = 0;
             X = atoi(stringData);
             indexStringData=0;
-            vx = ((float)X-(float)beforeX) / (float)timer.read_us() *1000;
+            vx = ((float)X-(float)beforeX) / (float)timer_v.read_us() *1000;
         }
 
         else if (val == 'Y') {
@@ -227,15 +199,13 @@
             stringData[indexStringData] = 0;
             Y = atoi(stringData);
             indexStringData=0;
-            //timer.stop();
+            //timer_v.stop();
             //newSpeedReady = true;
             //if( (Y-beforeY) > 5){ 
-            vy = ((float)Y-(float)beforeY) / (float)timer.read_us() *1000;
-            theta=atan2(vy,vx);
-            timer.reset();
-            timer.start();
-            sint = -sin(theta);//cos(theta);
-            cost = -cos(theta);//-sin(theta);  
+            vy = ((float)Y-(float)beforeY) / (float)timer_v.read_us() *1000;
+            //theta=atan2(vy,vx);
+            //sint = -sin(theta);//cos(theta);
+            //cost = -cos(theta);//-sin(theta);  
             //}
             //if (cost > 0){
             //sint = -sint;//cos(thet      
@@ -263,12 +233,7 @@
             radious = atoi(stringData);
             indexStringData=0;
             //makeBuffer();
-        } else if (val == 'T') {
-            stringData[indexStringData] = 0;
-            ticktime = atoi(stringData);
-            indexStringData=0;
-            //makeBuffer();
-        } 
+        }
         // X value?
         /*else if (val=='x') {
             stringData[indexStringData] = 0 ;