Hiromasa Oku / Mbed 2 deprecated linearMirrorMotion1017

this version 10/17

Dependencies:   mbed

Fork of linearMirrorMotion by Alvaro Cassinelli

main.cpp@11:8a0fecc86705, 2013-03-29 (annotated)

Committer:
hiromasaoku
Date:
Fri Mar 29 08:55:10 2013 +0000
Revision:
11:8a0fecc86705
Parent:
10:a3dd8ec4be60
Child:
12:d945fb6d4988
letter moving like rotary encoder. strange calculation....

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 11:8a0fecc86705 6 InterruptIn clockEncoderPin(p14);
hiromasaoku 11:8a0fecc86705 7 DigitalIn directionPin(p15);
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 11:8a0fecc86705 22 float theta=0, dt_betwords=50000, 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 11:8a0fecc86705 42 ////for culculate rotary velocit800000000y --------------------------------------------------------
hiromasaoku 4:e00e709d7173 43
hiromasaoku 10:a3dd8ec4be60 44 volatile float angleIncrement = 2.0*PI / 128; // when in Sign/Magnitude mode
hiromasaoku 11:8a0fecc86705 45 float radious = 700, radious_circle = 500;
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 11:8a0fecc86705 52 Ticker superEncoder;
hiromasaoku 5:e02cd57242c7 53
hiromasaoku 10:a3dd8ec4be60 54 // the external interrupt routine:
hiromasaoku 10:a3dd8ec4be60 55 void encoderClock() {
hiromasaoku 10:a3dd8ec4be60 56 if (directionPin) angle += angleIncrement ; else angle -= angleIncrement;
hiromasaoku 7:64b313c58420 57 }
hiromasaoku 7:64b313c58420 58
hiromasaoku 10:a3dd8ec4be60 59 void computeSpeed() {
hiromasaoku 10:a3dd8ec4be60 60 // We know exactly how much time passed since we last computed the speed, this is PERIODIC_COMPUTE in microseconds
hiromasaoku 11:8a0fecc86705 61 angularSpeed = ( 1000000.0 * (float)(angle-oldAngle) / (float)(PERIODIC_COMPUTE) + angularSpeed)/2; // in rad/sec
hiromasaoku 10:a3dd8ec4be60 62 oldAngle=angle;
hiromasaoku 10:a3dd8ec4be60 63 }
hiromasaoku 10:a3dd8ec4be60 64
hiromasaoku 10:a3dd8ec4be60 65 Timer timer;
hiromasaoku 10:a3dd8ec4be60 66
hiromasaoku 7:64b313c58420 67 int main(){
hiromasaoku 10:a3dd8ec4be60 68 //read from TextFileLibrary ------------------------------------------------------
hiromasaoku 9:4db47b8eb750 69 FILE *fp = fopen("/local/text.txt", "r");
hiromasaoku 5:e02cd57242c7 70 if(!fp) {
hiromasaoku 5:e02cd57242c7 71 IO.setGreenPower(1);
hiromasaoku 5:e02cd57242c7 72 exit(1);
hiromasaoku 9:4db47b8eb750 73 }
hiromasaoku 9:4db47b8eb750 74
hiromasaoku 4:e00e709d7173 75 int letternum;
hiromasaoku 4:e00e709d7173 76 fscanf(fp, "%d", &letternum);
hiromasaoku 5:e02cd57242c7 77 for(int i=0; i<letternum; i++) {
hiromasaoku 5:e02cd57242c7 78 letter bufl;
hiromasaoku 4:e00e709d7173 79 fscanf(fp, "%d", &bufl.pointnum);
hiromasaoku 5:e02cd57242c7 80 for(int j=0; j<bufl.pointnum; j++) {
hiromasaoku 5:e02cd57242c7 81 point2d bufp;
hiromasaoku 4:e00e709d7173 82 fscanf(fp, "%d", &bufp.x);
hiromasaoku 4:e00e709d7173 83 fscanf(fp, "%d", &bufp.y);
hiromasaoku 9:4db47b8eb750 84 fscanf(fp, "%d", &bufp.laserSwitch);
hiromasaoku 4:e00e709d7173 85 bufl.letpoints.push_back(bufp);
hiromasaoku 4:e00e709d7173 86 }
hiromasaoku 5:e02cd57242c7 87 libletter.push_back(bufl);
hiromasaoku 4:e00e709d7173 88 }
hiromasaoku 5:e02cd57242c7 89
hiromasaoku 5:e02cd57242c7 90
mbedalvaro 0:4e12dea53fbe 91 // SETUP: --------------------------------------------------------------------------------------------
mbedalvaro 0:4e12dea53fbe 92 IO.init(); // note: serial speed can be changed by checking in the hardwareIO.cpp initialization
hiromasaoku 4:e00e709d7173 93
hiromasaoku 10:a3dd8ec4be60 94 // initialize the angle (arbitrary origin):
hiromasaoku 10:a3dd8ec4be60 95 oldAngle=angle=0;
hiromasaoku 10:a3dd8ec4be60 96
hiromasaoku 10:a3dd8ec4be60 97 // Attach the external interrupt routine:
hiromasaoku 11:8a0fecc86705 98 //clockEncoderPin.rise(&encoderClock);
hiromasaoku 11:8a0fecc86705 99 //clockEncoderPin.fall(&encoderClock);
hiromasaoku 11:8a0fecc86705 100 superEncoder.attach_us(&encoderClock, radious_circle);
hiromasaoku 10:a3dd8ec4be60 101 timer.reset(); timer.start();
hiromasaoku 10:a3dd8ec4be60 102
hiromasaoku 10:a3dd8ec4be60 103 // Attach the periodic computing function:
hiromasaoku 10:a3dd8ec4be60 104 speedTimerCompute.attach_us(&computeSpeed, PERIODIC_COMPUTE);
hiromasaoku 10:a3dd8ec4be60 105
hiromasaoku 4:e00e709d7173 106 // Set displaying laser powers:
mbedalvaro 0:4e12dea53fbe 107 IO.setRedPower(0);
hiromasaoku 9:4db47b8eb750 108 IO.setGreenPower(0);
mbedalvaro 0:4e12dea53fbe 109 wait_ms(100);
mbedalvaro 0:4e12dea53fbe 110
hiromasaoku 5:e02cd57242c7 111 X = beforeX = CENTER_AD_MIRROR_X;
hiromasaoku 5:e02cd57242c7 112 Y = beforeY = CENTER_AD_MIRROR_Y;
hiromasaoku 10:a3dd8ec4be60 113 timer_v.start();
mbedalvaro 0:4e12dea53fbe 114 // MAIN LOOP: --------------------------------------------------------------------------------------------
mbedalvaro 0:4e12dea53fbe 115 while(1) {
hiromasaoku 6:a82917b3b1aa 116 if (pc.readable()>0) processSerial();
hiromasaoku 6:a82917b3b1aa 117
hiromasaoku 11:8a0fecc86705 118 if(1/*start*/) {
hiromasaoku 10:a3dd8ec4be60 119
hiromasaoku 10:a3dd8ec4be60 120 timer_v.reset();
hiromasaoku 6:a82917b3b1aa 121 wait_us(5000);
hiromasaoku 10:a3dd8ec4be60 122 // send the speed on the serial port every 30 ms:
hiromasaoku 11:8a0fecc86705 123 if (timer.read_ms()>30) {
hiromasaoku 10:a3dd8ec4be60 124 pc.printf("Angular Speed = %4.2f\t Cumulative Angle = %4.2f\n" , angularSpeed, angle);
hiromasaoku 10:a3dd8ec4be60 125 timer.reset();
hiromasaoku 11:8a0fecc86705 126 }
hiromasaoku 10:a3dd8ec4be60 127
hiromasaoku 10:a3dd8ec4be60 128
hiromasaoku 10:a3dd8ec4be60 129 // drawing ///-----------------------------------------------
hiromasaoku 5:e02cd57242c7 130 for(int i=0; i<inputletters.size(); i++) {
hiromasaoku 9:4db47b8eb750 131
hiromasaoku 9:4db47b8eb750 132 for(int j=0; j<libletter[inputletters[i]-'a'].letpoints.size(); j++) {
hiromasaoku 6:a82917b3b1aa 133
hiromasaoku 6:a82917b3b1aa 134 if (pc.readable()>0) processSerial();
hiromasaoku 11:8a0fecc86705 135 sint = sin(angle);
hiromasaoku 11:8a0fecc86705 136 cost = cos(angle);
hiromasaoku 6:a82917b3b1aa 137
hiromasaoku 11:8a0fecc86705 138 int x = -0.8*((libletter[inputletters[i]-'a'].letpoints[j].x ) - radious*angularSpeed*timer_v.read_us()/st*j/1000); //this ZURE should be modifyed not by dt but Timer.
hiromasaoku 6:a82917b3b1aa 139 int y = (libletter[inputletters[i]-'a'].letpoints[j].y);
hiromasaoku 11:8a0fecc86705 140 IO.writeOutXY(X + radious*cost + sint*x + cost*y,Y + radious*sint - cost*x + sint*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 11:8a0fecc86705 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 11:8a0fecc86705 236 }else if (val == 'T') {
hiromasaoku 11:8a0fecc86705 237 stringData[indexStringData] = 0;
hiromasaoku 11:8a0fecc86705 238 radious_circle = atoi(stringData);
hiromasaoku 11:8a0fecc86705 239 indexStringData=0;
hiromasaoku 11:8a0fecc86705 240 //makeBuffer();
hiromasaoku 10:a3dd8ec4be60 241 }
hiromasaoku 4:e00e709d7173 242 // X value?
hiromasaoku 5:e02cd57242c7 243 /*else if (val=='x') {
hiromasaoku 4:e00e709d7173 244 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 245 omegaX=atoi(stringData);
hiromasaoku 4:e00e709d7173 246 indexStringData=0;
hiromasaoku 4:e00e709d7173 247 //newPositionReady=true;
hiromasaoku 4:e00e709d7173 248 }
hiromasaoku 4:e00e709d7173 249 // Y value?
hiromasaoku 4:e00e709d7173 250 else if (val=='y') {
hiromasaoku 4:e00e709d7173 251 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 252 omegaY=atoi(stringData);
hiromasaoku 4:e00e709d7173 253 indexStringData=0;
hiromasaoku 5:e02cd57242c7 254 makeBuffer();
hiromasaoku 4:e00e709d7173 255 newPositionReady=true;
hiromasaoku 4:e00e709d7173 256 }
hiromasaoku 4:e00e709d7173 257
hiromasaoku 4:e00e709d7173 258 else if (val=='g') {
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.setGreenPower(power);
hiromasaoku 4:e00e709d7173 263 } else if (val=='r') {
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.setRedPower(power);
hiromasaoku 4:e00e709d7173 268 } else if (val=='c') {
hiromasaoku 4:e00e709d7173 269 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 270 int power=atoi(stringData);
hiromasaoku 4:e00e709d7173 271 indexStringData=0;
hiromasaoku 4:e00e709d7173 272 IO.setRGBPower(power);
hiromasaoku 4:e00e709d7173 273 }
hiromasaoku 5:e02cd57242c7 274 */
hiromasaoku 4:e00e709d7173 275
hiromasaoku 6:a82917b3b1aa 276 }
mbedalvaro 0:4e12dea53fbe 277 }