Hiromasa Oku / Mbed 2 deprecated linearMirrorMotion1017

this version 10/17

Dependencies:   mbed

Fork of linearMirrorMotion by Alvaro Cassinelli

main.cpp@10:a3dd8ec4be60, 2013-03-25 (annotated)

Committer:
hiromasaoku
Date:
Mon Mar 25 15:54:31 2013 +0000
Revision:
10:a3dd8ec4be60
Parent:
9:4db47b8eb750
Child:
11:8a0fecc86705
rotary encoder decoder inserted.;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedalvaro 0:4e12dea53fbe 1 #include "mbed.h"
mbedalvaro 1:daf6b4939120 2 #include "laserProjectorHardware.h"
hiromasaoku 4:e00e709d7173 3 #include <vector>
hiromasaoku 7:64b313c58420 4 #define PI 3.141592
hiromasaoku 7:64b313c58420 5
hiromasaoku 10:a3dd8ec4be60 6 InterruptIn clockEncoderPin(p5);
hiromasaoku 10:a3dd8ec4be60 7 DigitalIn directionPin(p6);
mbedalvaro 0:4e12dea53fbe 8
mbedalvaro 0:4e12dea53fbe 9 void processSerial();
hiromasaoku 10:a3dd8ec4be60 10 Timer timer_v;
hiromasaoku 4:e00e709d7173 11 LocalFileSystem local("local");
hiromasaoku 5:e02cd57242c7 12 //Serial pc(USBTX, USBRX);
mbedalvaro 0:4e12dea53fbe 13
hiromasaoku 10:a3dd8ec4be60 14 unsigned int X, Y, T;
hiromasaoku 5:e02cd57242c7 15 unsigned int beforeX , beforeY;
hiromasaoku 4:e00e709d7173 16 int startX = CENTER_AD_MIRROR_X;
hiromasaoku 4:e00e709d7173 17 int startY = CENTER_AD_MIRROR_Y;
hiromasaoku 4:e00e709d7173 18
hiromasaoku 6:a82917b3b1aa 19 float sint=0, cost=1;
hiromasaoku 6:a82917b3b1aa 20
hiromasaoku 10:a3dd8ec4be60 21 float vx = 0, vy=0;
hiromasaoku 10:a3dd8ec4be60 22 float theta=0, dt_betwords=200, st=400;
hiromasaoku 10:a3dd8ec4be60 23 unsigned int dt=500, ticktime=600;
hiromasaoku 4:e00e709d7173 24
mbedalvaro 0:4e12dea53fbe 25 bool newPositionReady=false;
mbedalvaro 0:4e12dea53fbe 26 unsigned int counter=0;
hiromasaoku 5:e02cd57242c7 27 vector<char> inputletters;
mbedalvaro 0:4e12dea53fbe 28
hiromasaoku 6:a82917b3b1aa 29 bool start=false;
hiromasaoku 6:a82917b3b1aa 30
hiromasaoku 5:e02cd57242c7 31 struct point2d {
hiromasaoku 4:e00e709d7173 32 int x,y;
hiromasaoku 9:4db47b8eb750 33 int laserSwitch; // laser {1: on 0:off} until next step
hiromasaoku 4:e00e709d7173 34 };
hiromasaoku 5:e02cd57242c7 35 struct letter {
hiromasaoku 4:e00e709d7173 36 int pointnum;
hiromasaoku 5:e02cd57242c7 37 vector<point2d> letpoints;
hiromasaoku 4:e00e709d7173 38 };
hiromasaoku 4:e00e709d7173 39
hiromasaoku 5:e02cd57242c7 40 vector<letter> libletter; // letter library
hiromasaoku 5:e02cd57242c7 41
hiromasaoku 10:a3dd8ec4be60 42 ////for culculate rotary velocity --------------------------------------------------------
hiromasaoku 4:e00e709d7173 43
hiromasaoku 10:a3dd8ec4be60 44 volatile float angleIncrement = 2.0*PI / 128; // when in Sign/Magnitude mode
hiromasaoku 10:a3dd8ec4be60 45 float radious = 100;
hiromasaoku 4:e00e709d7173 46
hiromasaoku 10:a3dd8ec4be60 47 // a ticker function to compute the speed periodically:
hiromasaoku 10:a3dd8ec4be60 48 #define PERIODIC_COMPUTE 10000 // in us
hiromasaoku 10:a3dd8ec4be60 49 volatile float angularSpeed = 0;
hiromasaoku 10:a3dd8ec4be60 50 volatile float angle=0, oldAngle=0;
hiromasaoku 10:a3dd8ec4be60 51 Ticker speedTimerCompute;
hiromasaoku 5:e02cd57242c7 52
hiromasaoku 10:a3dd8ec4be60 53 // the external interrupt routine:
hiromasaoku 10:a3dd8ec4be60 54 void encoderClock() {
hiromasaoku 10:a3dd8ec4be60 55 if (directionPin) angle += angleIncrement ; else angle -= angleIncrement;
hiromasaoku 7:64b313c58420 56 }
hiromasaoku 7:64b313c58420 57
hiromasaoku 10:a3dd8ec4be60 58 void computeSpeed() {
hiromasaoku 10:a3dd8ec4be60 59 // We know exactly how much time passed since we last computed the speed, this is PERIODIC_COMPUTE in microseconds
hiromasaoku 10:a3dd8ec4be60 60 angularSpeed = 1000000.0 * (angle-oldAngle) / (float)(PERIODIC_COMPUTE); // in rad/sec
hiromasaoku 10:a3dd8ec4be60 61 oldAngle=angle;
hiromasaoku 10:a3dd8ec4be60 62 }
hiromasaoku 10:a3dd8ec4be60 63
hiromasaoku 10:a3dd8ec4be60 64 Timer timer;
hiromasaoku 10:a3dd8ec4be60 65
hiromasaoku 7:64b313c58420 66 int main(){
hiromasaoku 10:a3dd8ec4be60 67 //read from TextFileLibrary ------------------------------------------------------
hiromasaoku 9:4db47b8eb750 68 FILE *fp = fopen("/local/text.txt", "r");
hiromasaoku 5:e02cd57242c7 69 if(!fp) {
hiromasaoku 5:e02cd57242c7 70 IO.setGreenPower(1);
hiromasaoku 5:e02cd57242c7 71 exit(1);
hiromasaoku 9:4db47b8eb750 72 }
hiromasaoku 9:4db47b8eb750 73
hiromasaoku 4:e00e709d7173 74 int letternum;
hiromasaoku 4:e00e709d7173 75 fscanf(fp, "%d", &letternum);
hiromasaoku 5:e02cd57242c7 76 for(int i=0; i<letternum; i++) {
hiromasaoku 5:e02cd57242c7 77 letter bufl;
hiromasaoku 4:e00e709d7173 78 fscanf(fp, "%d", &bufl.pointnum);
hiromasaoku 5:e02cd57242c7 79 for(int j=0; j<bufl.pointnum; j++) {
hiromasaoku 5:e02cd57242c7 80 point2d bufp;
hiromasaoku 4:e00e709d7173 81 fscanf(fp, "%d", &bufp.x);
hiromasaoku 4:e00e709d7173 82 fscanf(fp, "%d", &bufp.y);
hiromasaoku 9:4db47b8eb750 83 fscanf(fp, "%d", &bufp.laserSwitch);
hiromasaoku 4:e00e709d7173 84 bufl.letpoints.push_back(bufp);
hiromasaoku 4:e00e709d7173 85 }
hiromasaoku 5:e02cd57242c7 86 libletter.push_back(bufl);
hiromasaoku 4:e00e709d7173 87 }
hiromasaoku 5:e02cd57242c7 88
hiromasaoku 5:e02cd57242c7 89
mbedalvaro 0:4e12dea53fbe 90 // SETUP: --------------------------------------------------------------------------------------------
mbedalvaro 0:4e12dea53fbe 91 IO.init(); // note: serial speed can be changed by checking in the hardwareIO.cpp initialization
hiromasaoku 4:e00e709d7173 92
hiromasaoku 10:a3dd8ec4be60 93 // initialize the angle (arbitrary origin):
hiromasaoku 10:a3dd8ec4be60 94 oldAngle=angle=0;
hiromasaoku 10:a3dd8ec4be60 95
hiromasaoku 10:a3dd8ec4be60 96 // Attach the external interrupt routine:
hiromasaoku 10:a3dd8ec4be60 97 clockEncoderPin.rise(&encoderClock);
hiromasaoku 10:a3dd8ec4be60 98 timer.reset(); timer.start();
hiromasaoku 10:a3dd8ec4be60 99
hiromasaoku 10:a3dd8ec4be60 100 // Attach the periodic computing function:
hiromasaoku 10:a3dd8ec4be60 101 speedTimerCompute.attach_us(&computeSpeed, PERIODIC_COMPUTE);
hiromasaoku 10:a3dd8ec4be60 102
hiromasaoku 4:e00e709d7173 103 // Set displaying laser powers:
mbedalvaro 0:4e12dea53fbe 104 IO.setRedPower(0);
hiromasaoku 9:4db47b8eb750 105 IO.setGreenPower(0);
mbedalvaro 0:4e12dea53fbe 106 wait_ms(100);
mbedalvaro 0:4e12dea53fbe 107
hiromasaoku 5:e02cd57242c7 108 X = beforeX = CENTER_AD_MIRROR_X;
hiromasaoku 5:e02cd57242c7 109 Y = beforeY = CENTER_AD_MIRROR_Y;
hiromasaoku 10:a3dd8ec4be60 110 timer_v.start();
mbedalvaro 0:4e12dea53fbe 111 // MAIN LOOP: --------------------------------------------------------------------------------------------
mbedalvaro 0:4e12dea53fbe 112 while(1) {
hiromasaoku 6:a82917b3b1aa 113 if (pc.readable()>0) processSerial();
hiromasaoku 6:a82917b3b1aa 114
hiromasaoku 6:a82917b3b1aa 115 if(start) {
hiromasaoku 10:a3dd8ec4be60 116
hiromasaoku 10:a3dd8ec4be60 117 timer_v.reset();
hiromasaoku 6:a82917b3b1aa 118 wait_us(5000);
hiromasaoku 10:a3dd8ec4be60 119 //culculating velocity -----------------------------------
hiromasaoku 10:a3dd8ec4be60 120 //velocity valuable :: sint, cost, theta, vx, vy
hiromasaoku 10:a3dd8ec4be60 121
hiromasaoku 10:a3dd8ec4be60 122 // send the speed on the serial port every 30 ms:
hiromasaoku 10:a3dd8ec4be60 123 if (timer.read_ms()>1000) {
hiromasaoku 10:a3dd8ec4be60 124 pc.printf("Angular Speed = %4.2f\t Cumulative Angle = %4.2f\n" , angularSpeed, angle);
hiromasaoku 10:a3dd8ec4be60 125 sint = sin(angle);
hiromasaoku 10:a3dd8ec4be60 126 cost = cos(angle);
hiromasaoku 10:a3dd8ec4be60 127 timer.reset();
hiromasaoku 10:a3dd8ec4be60 128 }
hiromasaoku 10:a3dd8ec4be60 129
hiromasaoku 10:a3dd8ec4be60 130
hiromasaoku 10:a3dd8ec4be60 131 // drawing ///-----------------------------------------------
hiromasaoku 5:e02cd57242c7 132 for(int i=0; i<inputletters.size(); i++) {
hiromasaoku 9:4db47b8eb750 133
hiromasaoku 9:4db47b8eb750 134 for(int j=0; j<libletter[inputletters[i]-'a'].letpoints.size(); j++) {
hiromasaoku 6:a82917b3b1aa 135
hiromasaoku 6:a82917b3b1aa 136 if (pc.readable()>0) processSerial();
hiromasaoku 6:a82917b3b1aa 137
hiromasaoku 10:a3dd8ec4be60 138 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.
hiromasaoku 6:a82917b3b1aa 139 int y = (libletter[inputletters[i]-'a'].letpoints[j].y);
hiromasaoku 6:a82917b3b1aa 140 IO.writeOutXY(X+cost*x - sint*y,Y + sint*x + cost*y);
hiromasaoku 10:a3dd8ec4be60 141 timer_v.reset();
hiromasaoku 5:e02cd57242c7 142 wait_us(dt);
hiromasaoku 9:4db47b8eb750 143 IO.setRedPower(libletter[inputletters[i]-'a'].letpoints[j].laserSwitch);//on
hiromasaoku 5:e02cd57242c7 144 }
hiromasaoku 9:4db47b8eb750 145 IO.setRedPower(0); //off
hiromasaoku 9:4db47b8eb750 146 wait_us(dt_betwords);//sqrtf(vx*vx+vy*vy)*100000);
hiromasaoku 6:a82917b3b1aa 147
hiromasaoku 5:e02cd57242c7 148 }
hiromasaoku 10:a3dd8ec4be60 149 ////////////////
hiromasaoku 6:a82917b3b1aa 150 IO.setRGBPower(1); //off
hiromasaoku 6:a82917b3b1aa 151 wait_us(10000);
hiromasaoku 6:a82917b3b1aa 152 start=false;
hiromasaoku 6:a82917b3b1aa 153
hiromasaoku 6:a82917b3b1aa 154 }
hiromasaoku 6:a82917b3b1aa 155
hiromasaoku 10:a3dd8ec4be60 156
hiromasaoku 4:e00e709d7173 157
mbedalvaro 0:4e12dea53fbe 158 }
mbedalvaro 0:4e12dea53fbe 159 }
mbedalvaro 0:4e12dea53fbe 160
hiromasaoku 4:e00e709d7173 161
mbedalvaro 0:4e12dea53fbe 162 // --------------------------------------------------------------------------------------------
hiromasaoku 4:e00e709d7173 163 // String to store ALPHANUMERIC DATA (i.e., integers, floating point numbers, unsigned ints, etc represented as DEC) sent wirelessly:
mbedalvaro 0:4e12dea53fbe 164 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...
mbedalvaro 0:4e12dea53fbe 165 int indexStringData=0;//position of the byte in the string
mbedalvaro 0:4e12dea53fbe 166
hiromasaoku 4:e00e709d7173 167 void processSerial()
hiromasaoku 4:e00e709d7173 168 {
hiromasaoku 4:e00e709d7173 169
hiromasaoku 6:a82917b3b1aa 170 start=true;
hiromasaoku 4:e00e709d7173 171 while(pc.readable()>0) {
hiromasaoku 4:e00e709d7173 172
hiromasaoku 4:e00e709d7173 173 char val =pc.getc();
hiromasaoku 4:e00e709d7173 174
hiromasaoku 4:e00e709d7173 175 // Save ASCII numeric characters (ASCII 0 - 9) on stringData:
hiromasaoku 4:e00e709d7173 176 if ((val >= '0') && (val <= '9')) { // this is 45 to 57 (included)
hiromasaoku 4:e00e709d7173 177 stringData[indexStringData] = val;
hiromasaoku 4:e00e709d7173 178 indexStringData++;
hiromasaoku 5:e02cd57242c7 179 } else if ((val >= 'a') && (val <= 'z')) { // this is 45 to 57 (included)
hiromasaoku 5:e02cd57242c7 180 inputletters.push_back(val);
hiromasaoku 5:e02cd57242c7 181 }
hiromasaoku 5:e02cd57242c7 182
hiromasaoku 5:e02cd57242c7 183 /*else if (val == '/') {
hiromasaoku 5:e02cd57242c7 184 makeBuffer();
hiromasaoku 5:e02cd57242c7 185 }*/ else if (val == '.') {
hiromasaoku 5:e02cd57242c7 186 inputletters.clear();
hiromasaoku 4:e00e709d7173 187 }
mbedalvaro 0:4e12dea53fbe 188
hiromasaoku 4:e00e709d7173 189 else if (val == 'X') {
hiromasaoku 5:e02cd57242c7 190 beforeX = X;
hiromasaoku 4:e00e709d7173 191 stringData[indexStringData] = 0;
hiromasaoku 5:e02cd57242c7 192 X = atoi(stringData);
hiromasaoku 4:e00e709d7173 193 indexStringData=0;
hiromasaoku 10:a3dd8ec4be60 194 vx = ((float)X-(float)beforeX) / (float)timer_v.read_us() *1000;
hiromasaoku 4:e00e709d7173 195 }
hiromasaoku 4:e00e709d7173 196
hiromasaoku 4:e00e709d7173 197 else if (val == 'Y') {
hiromasaoku 5:e02cd57242c7 198 beforeY = Y;
hiromasaoku 4:e00e709d7173 199 stringData[indexStringData] = 0;
hiromasaoku 5:e02cd57242c7 200 Y = atoi(stringData);
hiromasaoku 4:e00e709d7173 201 indexStringData=0;
hiromasaoku 10:a3dd8ec4be60 202 //timer_v.stop();
hiromasaoku 4:e00e709d7173 203 //newSpeedReady = true;
hiromasaoku 6:a82917b3b1aa 204 //if( (Y-beforeY) > 5){
hiromasaoku 10:a3dd8ec4be60 205 vy = ((float)Y-(float)beforeY) / (float)timer_v.read_us() *1000;
hiromasaoku 10:a3dd8ec4be60 206 //theta=atan2(vy,vx);
hiromasaoku 10:a3dd8ec4be60 207 //sint = -sin(theta);//cos(theta);
hiromasaoku 10:a3dd8ec4be60 208 //cost = -cos(theta);//-sin(theta);
hiromasaoku 6:a82917b3b1aa 209 //}
hiromasaoku 6:a82917b3b1aa 210 //if (cost > 0){
hiromasaoku 6:a82917b3b1aa 211 //sint = -sint;//cos(thet
hiromasaoku 6:a82917b3b1aa 212 //cost = -cost;//-sin(theta);
hiromasaoku 6:a82917b3b1aa 213 //}
hiromasaoku 4:e00e709d7173 214 }
mbedalvaro 2:0548c7bf9fba 215
hiromasaoku 7:64b313c58420 216 else if (val == 'D') {
hiromasaoku 4:e00e709d7173 217 stringData[indexStringData] = 0;
hiromasaoku 4:e00e709d7173 218 dt = atoi(stringData);
hiromasaoku 4:e00e709d7173 219 indexStringData=0;
hiromasaoku 5:e02cd57242c7 220 //makeBuffer();
hiromasaoku 7:64b313c58420 221 } else if (val == 'B') {
hiromasaoku 7:64b313c58420 222 stringData[indexStringData] = 0;
hiromasaoku 7:64b313c58420 223 dt_betwords = atoi(stringData);
hiromasaoku 7:64b313c58420 224 indexStringData=0;
hiromasaoku 7:64b313c58420 225 //makeBuffer();
hiromasaoku 5:e02cd57242c7 226 } else if (val == 'S') {
hiromasaoku 4:e00e709d7173 227 stringData[indexStringData] = 0;
hiromasaoku 7:64b313c58420 228 st = atoi(stringData);
hiromasaoku 7:64b313c58420 229 indexStringData=0;
hiromasaoku 7:64b313c58420 230 //makeBuffer();
hiromasaoku 7:64b313c58420 231 }else if (val == 'R') {
hiromasaoku 7:64b313c58420 232 stringData[indexStringData] = 0;
hiromasaoku 7:64b313c58420 233 radious = atoi(stringData);
hiromasaoku 7:64b313c58420 234 indexStringData=0;
hiromasaoku 7:64b313c58420 235 //makeBuffer();
hiromasaoku 10:a3dd8ec4be60 236 }
hiromasaoku 4:e00e709d7173 237 // X value?
hiromasaoku 5:e02cd57242c7 238 /*else if (val=='x') {
hiromasaoku 4:e00e709d7173 239 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 240 omegaX=atoi(stringData);
hiromasaoku 4:e00e709d7173 241 indexStringData=0;
hiromasaoku 4:e00e709d7173 242 //newPositionReady=true;
hiromasaoku 4:e00e709d7173 243 }
hiromasaoku 4:e00e709d7173 244 // Y value?
hiromasaoku 4:e00e709d7173 245 else if (val=='y') {
hiromasaoku 4:e00e709d7173 246 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 247 omegaY=atoi(stringData);
hiromasaoku 4:e00e709d7173 248 indexStringData=0;
hiromasaoku 5:e02cd57242c7 249 makeBuffer();
hiromasaoku 4:e00e709d7173 250 newPositionReady=true;
hiromasaoku 4:e00e709d7173 251 }
hiromasaoku 4:e00e709d7173 252
hiromasaoku 4:e00e709d7173 253 else if (val=='g') {
hiromasaoku 4:e00e709d7173 254 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 255 int power=atoi(stringData);
hiromasaoku 4:e00e709d7173 256 indexStringData=0;
hiromasaoku 4:e00e709d7173 257 IO.setGreenPower(power);
hiromasaoku 4:e00e709d7173 258 } else if (val=='r') {
hiromasaoku 4:e00e709d7173 259 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 260 int power=atoi(stringData);
hiromasaoku 4:e00e709d7173 261 indexStringData=0;
hiromasaoku 4:e00e709d7173 262 IO.setRedPower(power);
hiromasaoku 4:e00e709d7173 263 } else if (val=='c') {
hiromasaoku 4:e00e709d7173 264 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 265 int power=atoi(stringData);
hiromasaoku 4:e00e709d7173 266 indexStringData=0;
hiromasaoku 4:e00e709d7173 267 IO.setRGBPower(power);
hiromasaoku 4:e00e709d7173 268 }
hiromasaoku 5:e02cd57242c7 269 */
hiromasaoku 4:e00e709d7173 270
hiromasaoku 6:a82917b3b1aa 271 }
mbedalvaro 0:4e12dea53fbe 272 }