Diff: main.cpp

Revision:

7:64b313c58420

Parent:

6:a82917b3b1aa

Child:

8:617dbd2b6512

diff -r a82917b3b1aa -r 64b313c58420 main.cpp
--- a/main.cpp	Wed Dec 05 08:04:25 2012 +0000
+++ b/main.cpp	Mon Dec 10 07:14:59 2012 +0000
@@ -10,6 +10,9 @@
 #include "mbed.h"
 #include "laserProjectorHardware.h"
 #include <vector>
+#define PI 3.141592
+
+Ticker changeXY;
 
 void processSerial();
 Timer timer;
@@ -23,9 +26,9 @@
 
 float sint=0, cost=1;
           
-float vx = 0, vy=0, omegaX=0, omegaY=0; 
-float theta=0,scale=7000;
-unsigned int dt=500, timecounter = 0;
+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;
 
 bool newPositionReady=false;
 unsigned int counter=0;
@@ -63,8 +66,8 @@
         buf.push_back(222);
         
         for(int j=0; j<libletter[inputletters[i]-'a'].letpoints.size(); j++) {
-            int x = 0.8*scale/1000*(libletter[inputletters[i]-'a'].letpoints[j].x +500*(i-inputletters.size()/2.0));
-            int y = -scale/1000*(libletter[inputletters[i]-'a'].letpoints[j].y);
+            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);
         }
@@ -73,10 +76,19 @@
 }
 */
 
-int main()
-{
-    
-    
+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 ++;
+}
+
+int main(){
+
     FILE *fp = fopen("/local/test.txt", "r");
     if(!fp) {
         IO.setGreenPower(1);
@@ -108,15 +120,16 @@
 
     wait_ms(100);
 
-    
     X = beforeX = CENTER_AD_MIRROR_X;
     Y = beforeY = CENTER_AD_MIRROR_Y;
     timer.start();
+    changeXY.attach_us(&CircleXY, 200);
     // MAIN LOOP: --------------------------------------------------------------------------------------------
     while(1) {
     if (pc.readable()>0) processSerial();
     
     if(start) {
+        timer.reset();
         wait_us(5000);
         for(int i=0; i<inputletters.size(); i++) {
     
@@ -124,14 +137,15 @@
             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(fabs(vx*vy))*dt/10000*j); //+500*(i-inputletters.size()/2.0));
+                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 y = (libletter[inputletters[i]-'a'].letpoints[j].y);
                 IO.writeOutXY(X+cost*x - sint*y,Y + sint*x + cost*y);
+                timer.reset();
                 wait_us(dt);
                 IO.setRGBPower(3);//on
             }
             IO.setRGBPower(1); //off
-            wait_us(scale);//10*dt);
+            wait_us(dt_betwords/sqrtf(vx*vx+vy*vy)*100000);
 
         }
         
@@ -203,12 +217,7 @@
             stringData[indexStringData] = 0;
             X = atoi(stringData);
             indexStringData=0;
-            //timer.stop();
-            vx = ((float)X-(float)beforeX) / (float)timer.read_ms();
-            //newSpeedReady = true;
-            //theta=atan2(vy,vx);
-            //timer.reset();
-            //timer.start();
+            vx = ((float)X-(float)beforeX) / (float)timer.read_us() *1000;
         }
 
         else if (val == 'Y') {
@@ -219,7 +228,7 @@
             //timer.stop();
             //newSpeedReady = true;
             //if( (Y-beforeY) > 5){ 
-            vy = ((float)Y-(float)beforeY) / (float)timer.read_ms();
+            vy = ((float)Y-(float)beforeY) / (float)timer.read_us() *1000;
             theta=atan2(vy,vx);
             timer.reset();
             timer.start();
@@ -232,14 +241,29 @@
             //}
         }
 
-        else if (val == 'T') {
+        else if (val == 'D') {
             stringData[indexStringData] = 0;
             dt = atoi(stringData);
             indexStringData=0;
             //makeBuffer();
+        } else if (val == 'B') {
+            stringData[indexStringData] = 0;
+            dt_betwords = atoi(stringData);
+            indexStringData=0;
+            //makeBuffer();
         } else if (val == 'S') {
             stringData[indexStringData] = 0;
-            scale = atoi(stringData);
+            st = atoi(stringData);
+            indexStringData=0;
+            //makeBuffer();
+        }else if (val == 'R') {
+            stringData[indexStringData] = 0;
+            radious = atoi(stringData);
+            indexStringData=0;
+            //makeBuffer();
+        } else if (val == 'T') {
+            stringData[indexStringData] = 0;
+            ticktime = atoi(stringData);
             indexStringData=0;
             //makeBuffer();
         }