File content as of revision 12:d945fb6d4988:

#include "mbed.h"
#include "laserProjectorHardware.h"
#include <vector>
#define PI 3.141592

InterruptIn clockEncoderPin(p14);
DigitalIn directionPin(p15);

void processSerial();
Timer timer_v;
LocalFileSystem local("local");
//Serial pc(USBTX, USBRX);

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;
float theta=0, dt_betwords=50000, st=400;
unsigned int dt=500, ticktime=600;

bool newPositionReady=false;
unsigned int counter=0;
vector<char> inputletters;

bool start=false;

struct point2dl {
    int x,y;
    int laserSwitch;   // laser {1: on 0:off} until next step
};

point2dl shearing(point2dl dataBefore, float radious, float angularSpeed){
    point2dl dataAfter;
    dataAfter.x = dataBefore.x - radious*angularSpeed*timer_v.read_us()/st/1000;
    dataAfter.y = dataBefore.y - radious*angularSpeed*timer_v.read_us()/st/1000;
    dataAfter.laserSwitch = dataBefore.laserSwitch;
    
    return dataAfter;
}

point2dl rotation(point2dl dataBefore, float theta){
    sint = sin(theta);
    cost = cos(theta);
    point2dl dataAfter;
    dataAfter.x = sint*dataBefore.x + cost*dataBefore.y;
    dataAfter.y = -cost*dataBefore.x + sint*dataBefore.y;
    dataAfter.laserSwitch = dataBefore.laserSwitch;

    return dataAfter;
}

struct letter {
    int pointnum;
    vector<point2dl> letpoints;
};

vector<letter> libletter;   // letter library

////for culculate rotary velocit800000000y --------------------------------------------------------

volatile float angleIncrement = 2.0*PI / 128; // when in Sign/Magnitude mode
float radious = 700, radious_circle = 500;

// 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;
Ticker superEncoder;

// 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 * (float)(angle-oldAngle) / (float)(PERIODIC_COMPUTE) + angularSpeed)/2; // in rad/sec
    oldAngle=angle;
}
   
Timer timer;

int main(){
    //read from TextFileLibrary ------------------------------------------------------
    FILE *fp = fopen("/local/text.txt", "r");
    if(!fp) {
        IO.setGreenPower(1);
        exit(1);
    }   
    
    int letternum;
    fscanf(fp, "%d", &letternum);
    for(int i=0; i<letternum; i++) {
        letter bufl;
        fscanf(fp, "%d", &bufl.pointnum);
        for(int j=0; j<bufl.pointnum; j++) {
            point2dl bufp;
            fscanf(fp, "%d", &bufp.x);
            fscanf(fp, "%d", &bufp.y);
            fscanf(fp, "%d", &bufp.laserSwitch);
            bufl.letpoints.push_back(bufp);
        }
        libletter.push_back(bufl);
    }


    // 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);
    //clockEncoderPin.fall(&encoderClock);
    superEncoder.attach_us(&encoderClock, radious_circle);
    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);
    wait_ms(100);

    X = beforeX = CENTER_AD_MIRROR_X;
    Y = beforeY = CENTER_AD_MIRROR_Y;
    timer_v.start();
    // MAIN LOOP: --------------------------------------------------------------------------------------------
    while(1) {
    if (pc.readable()>0) processSerial();
    
    if(1/*start*/) {
        
        timer_v.reset();
        wait_us(5000);
        // send the speed on the serial port every 30 ms:
        if (timer.read_ms()>30) {
            pc.printf("Angular Speed = %4.2f\t Cumulative Angle = %4.2f\n" , angularSpeed, 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();
                
                point2dl sheared = shearing(libletter[inputletters[i]-'a'].letpoints[j] , radious , angularSpeed );
                point2dl rotated = rotation(sheared, angle );
                IO.writeOutXY(X + rotated.x,Y + rotated.y );
                
                /*
                int x = -0.8*((libletter[inputletters[i]-'a'].letpoints[j].x ) - radious*angularSpeed*timer_v.read_us()/st/1000); //this ZURE should be modifyed not by dt but Timer.
                int y = (libletter[inputletters[i]-'a'].letpoints[j].y);
                IO.writeOutXY(X + radious*cost + sint*x + cost*y,Y + radious*sint - cost*x + sint*y);
                */
                
                wait_us(dt);
                IO.setRedPower(libletter[inputletters[i]-'a'].letpoints[j].laserSwitch);//on
            }
            timer_v.reset();
            IO.setRedPower(0); //off
            wait_us(dt_betwords);//sqrtf(vx*vx+vy*vy)*100000);

        }
        ////////////////
        IO.setRGBPower(1); //off
        wait_us(10000);
        start=false;
        ////////////////////*/
        }
        
        

    }
}


// --------------------------------------------------------------------------------------------
// String to store ALPHANUMERIC DATA (i.e., integers, floating point numbers, unsigned ints, etc represented as DEC) sent wirelessly:
char stringData[24]; // note: an integer is two bytes long, represented with a maximum of 5 digits, but we may send floats or unsigned int...
int indexStringData=0;//position of the byte in the string

void processSerial()
{

    start=true;
    while(pc.readable()>0) {

        char val =pc.getc();

        // Save ASCII numeric characters (ASCII 0 - 9) on stringData:
        if ((val >= '0') && (val <= '9')) { // this is 45 to 57 (included)
            stringData[indexStringData] = val;
            indexStringData++;
        } else if ((val >= 'a') && (val <= 'z')) { // this is 45 to 57 (included)
            inputletters.push_back(val);
        }

        /*else if (val == '/') {
            makeBuffer();
        }*/ else if (val == '.') {
            inputletters.clear();
        }

        else if (val == 'X') {
            beforeX = X;
            stringData[indexStringData] = 0;
            X = atoi(stringData);
            indexStringData=0;
            vx = ((float)X-(float)beforeX) / (float)timer_v.read_us() *1000;
        }

        else if (val == 'Y') {
            beforeY = Y;
            stringData[indexStringData] = 0;
            Y = atoi(stringData);
            indexStringData=0;
            //timer_v.stop();
            //newSpeedReady = true;
            //if( (Y-beforeY) > 5){ 
            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      
            //cost = -cost;//-sin(theta);  
            //}
        }

        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;
            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;
            radious_circle = atoi(stringData);
            indexStringData=0;
            //makeBuffer();
        }
        // X value?
        /*else if (val=='x') {
            stringData[indexStringData] = 0 ;
            omegaX=atoi(stringData);
            indexStringData=0;
            //newPositionReady=true;
        }
        // Y value?
        else if (val=='y') {
            stringData[indexStringData] = 0 ;
            omegaY=atoi(stringData);
            indexStringData=0;
            makeBuffer();
            newPositionReady=true;
        }

        else if (val=='g') {
            stringData[indexStringData] = 0 ;
            int power=atoi(stringData);
            indexStringData=0;
            IO.setGreenPower(power);
        } else if (val=='r') {
            stringData[indexStringData] = 0 ;
            int power=atoi(stringData);
            indexStringData=0;
            IO.setRedPower(power);
        } else if (val=='c') {
            stringData[indexStringData] = 0 ;
            int power=atoi(stringData);
            indexStringData=0;
            IO.setRGBPower(power);
        }
        */

   }
}