Takashi Asano / Mbed 2 deprecated linearMirrorMotion1017

sotsuron

Fork of linearMirrorMotion1017 by Hiromasa Oku

main.cpp@29:aa8fe9fccec8, 2013-07-30 (annotated)

Committer:
takapiasano
Date:
Tue Jul 30 04:16:21 2013 +0000
Revision:
29:aa8fe9fccec8
Parent:
20:8475768fc2f7 
close to complete

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedalvaro 0:4e12dea53fbe 1 #include "mbed.h"
hiromasaoku 19:8e9fe7d25b9c 2 #include <string>
hiromasaoku 16:6dd2e60bc5bc 3 #include <vector>
hiromasaoku 16:6dd2e60bc5bc 4 #include "renderclass.h"
mbedalvaro 1:daf6b4939120 5 #include "laserProjectorHardware.h"
hiromasaoku 7:64b313c58420 6
hiromasaoku 17:dce982e0a383 7 render myRender;
hiromasaoku 17:dce982e0a383 8
hiromasaoku 19:8e9fe7d25b9c 9 InterruptIn clockEncoderPin(p14); //blue
hiromasaoku 19:8e9fe7d25b9c 10 DigitalIn directionPin(p15); //green
mbedalvaro 0:4e12dea53fbe 11
takapiasano 29:aa8fe9fccec8 12 AnalogIn iny(p20);
takapiasano 29:aa8fe9fccec8 13 AnalogIn inx(p19);
takapiasano 29:aa8fe9fccec8 14 AnalogIn inPotX(p16);
takapiasano 29:aa8fe9fccec8 15 AnalogIn inPotY(p17);
takapiasano 29:aa8fe9fccec8 16
mbedalvaro 0:4e12dea53fbe 17 void processSerial();
takapiasano 29:aa8fe9fccec8 18 //Timer timer_v; // for shearing //for send the speed on the serial port every 30 ms:
hiromasaoku 4:e00e709d7173 19 LocalFileSystem local("local");
hiromasaoku 5:e02cd57242c7 20 //Serial pc(USBTX, USBRX);
mbedalvaro 0:4e12dea53fbe 21
hiromasaoku 10:a3dd8ec4be60 22 unsigned int X, Y, T;
hiromasaoku 5:e02cd57242c7 23 unsigned int beforeX , beforeY;
hiromasaoku 4:e00e709d7173 24 int startX = CENTER_AD_MIRROR_X;
hiromasaoku 4:e00e709d7173 25 int startY = CENTER_AD_MIRROR_Y;
hiromasaoku 4:e00e709d7173 26
hiromasaoku 6:a82917b3b1aa 27 float sint=0, cost=1;
hiromasaoku 17:dce982e0a383 28
hiromasaoku 10:a3dd8ec4be60 29 float vx = 0, vy=0;
hiromasaoku 14:314b86828ed2 30 float theta=0, dt_betWords=50000, st=1800;
takapiasano 29:aa8fe9fccec8 31 int laserColor=1, ticktime=600;
takapiasano 29:aa8fe9fccec8 32 int potxx = 0,potyy = 0;
takapiasano 29:aa8fe9fccec8 33
hiromasaoku 4:e00e709d7173 34
mbedalvaro 0:4e12dea53fbe 35 bool newPositionReady=false;
mbedalvaro 0:4e12dea53fbe 36 unsigned int counter=0;
hiromasaoku 5:e02cd57242c7 37 vector<char> inputletters;
mbedalvaro 0:4e12dea53fbe 38
hiromasaoku 6:a82917b3b1aa 39 bool start=false;
hiromasaoku 6:a82917b3b1aa 40
hiromasaoku 12:d945fb6d4988 41
hiromasaoku 16:6dd2e60bc5bc 42 //renderclass----------------------------------------------------------
hiromasaoku 12:d945fb6d4988 43
hiromasaoku 16:6dd2e60bc5bc 44 vector<letter> alphabet; // letter library
hiromasaoku 16:6dd2e60bc5bc 45 vector<letter> myText;
hiromasaoku 12:d945fb6d4988 46
hiromasaoku 19:8e9fe7d25b9c 47 /*
hiromasaoku 19:8e9fe7d25b9c 48 string textString="H";
hiromasaoku 16:6dd2e60bc5bc 49
hiromasaoku 19:8e9fe7d25b9c 50 void createTextPoints(string& _str) {
hiromasaoku 19:8e9fe7d25b9c 51 myText.clear();
hiromasaoku 19:8e9fe7d25b9c 52 for (int i=0; i<_str.length(); i++) {
hiromasaoku 19:8e9fe7d25b9c 53 myText.push_back(alphabet[(int)_str[i]-'a']);
hiromasaoku 19:8e9fe7d25b9c 54 }
hiromasaoku 19:8e9fe7d25b9c 55 }
hiromasaoku 19:8e9fe7d25b9c 56 */
hiromasaoku 12:d945fb6d4988 57
hiromasaoku 16:6dd2e60bc5bc 58 ////for culculate rotary velocity --------------------------------------------------------
takapiasano 29:aa8fe9fccec8 59 int radious = 700, attachSecond = 500;
takapiasano 29:aa8fe9fccec8 60 /*volatile float*/ int angleIncrement = 1;//2.0*PI / 2500;// when in Sign/Magnitude mode
hiromasaoku 4:e00e709d7173 61
hiromasaoku 16:6dd2e60bc5bc 62 // a ticker function to compute the speed periodically-------------------------------------------------------------
takapiasano 29:aa8fe9fccec8 63 #define PERIODIC_COMPUTE 5000 // in us
hiromasaoku 10:a3dd8ec4be60 64 volatile float angularSpeed = 0;
takapiasano 29:aa8fe9fccec8 65 float angleX=0.5;
takapiasano 29:aa8fe9fccec8 66 float angleY=0.5;
takapiasano 29:aa8fe9fccec8 67 float angleSpeedX=0;
takapiasano 29:aa8fe9fccec8 68 float angleSpeedY=0;
hiromasaoku 10:a3dd8ec4be60 69 Ticker speedTimerCompute;
hiromasaoku 11:8a0fecc86705 70 Ticker superEncoder;
hiromasaoku 5:e02cd57242c7 71
takapiasano 29:aa8fe9fccec8 72 float PotX, PotY;
takapiasano 29:aa8fe9fccec8 73
takapiasano 29:aa8fe9fccec8 74
hiromasaoku 10:a3dd8ec4be60 75 // the external interrupt routine:
hiromasaoku 17:dce982e0a383 76 void encoderClock()
hiromasaoku 17:dce982e0a383 77 {
takapiasano 29:aa8fe9fccec8 78 pc.printf("%f,%f,\n % f, %f \n\n" , PotX,PotY,angleX,angleY);
hiromasaoku 7:64b313c58420 79 }
hiromasaoku 7:64b313c58420 80
hiromasaoku 17:dce982e0a383 81 void computeSpeed()
hiromasaoku 17:dce982e0a383 82 {
takapiasano 29:aa8fe9fccec8 83 float oldAngleX = angleX;
takapiasano 29:aa8fe9fccec8 84 float oldAngleY = angleY;
takapiasano 29:aa8fe9fccec8 85
takapiasano 29:aa8fe9fccec8 86 angleX = inx;
takapiasano 29:aa8fe9fccec8 87 angleY = iny;
takapiasano 29:aa8fe9fccec8 88
takapiasano 29:aa8fe9fccec8 89 PotX = inPotX;//0.25*potxx;//
takapiasano 29:aa8fe9fccec8 90 PotY = inPotY;//0.25*potyy;//
takapiasano 29:aa8fe9fccec8 91
takapiasano 29:aa8fe9fccec8 92 // low pass filter (recusive filter):
takapiasano 29:aa8fe9fccec8 93 float alpha=.35; // alpha=dt/(RC+dt), with dt=COMPUTE_SPEED, and f_cut=1/(2*PI*RC) is the filter cutoff frequency
takapiasano 29:aa8fe9fccec8 94 // we want f_cut=1kHz, alpha=COMPUTE_SPEED/(1/(2.PI.fcut)+COMPUTE_SPEED) => alpha=31.415
takapiasano 29:aa8fe9fccec8 95 angleX=alpha*angleX+(1-alpha)*oldAngleX;
takapiasano 29:aa8fe9fccec8 96 angleY=alpha*angleY+(1-alpha)*oldAngleY;
takapiasano 29:aa8fe9fccec8 97
takapiasano 29:aa8fe9fccec8 98 if(abs(angleX - oldAngleX) <0.0005 || abs(angleY - oldAngleY) < 0.0005) myRender.startSwitch=1;
takapiasano 29:aa8fe9fccec8 99
takapiasano 29:aa8fe9fccec8 100 angleSpeedX = -(angleX - oldAngleX) * PotX;
takapiasano 29:aa8fe9fccec8 101 angleSpeedY = -(angleY - oldAngleY) * PotY;
takapiasano 29:aa8fe9fccec8 102
takapiasano 29:aa8fe9fccec8 103 //pc.printf("%f %f \n" ,angleX,angleY);
takapiasano 29:aa8fe9fccec8 104 //pc.printf("%f %f \n", PotX,PotY);
takapiasano 29:aa8fe9fccec8 105
takapiasano 29:aa8fe9fccec8 106 myRender.updateSpeed(angleSpeedX,angleSpeedY);
hiromasaoku 20:8475768fc2f7 107 void processSerial();
hiromasaoku 10:a3dd8ec4be60 108 }
hiromasaoku 17:dce982e0a383 109
hiromasaoku 10:a3dd8ec4be60 110
hiromasaoku 17:dce982e0a383 111 int main()
hiromasaoku 17:dce982e0a383 112 {
hiromasaoku 10:a3dd8ec4be60 113 //read from TextFileLibrary ------------------------------------------------------
hiromasaoku 9:4db47b8eb750 114 FILE *fp = fopen("/local/text.txt", "r");
hiromasaoku 5:e02cd57242c7 115 if(!fp) {
hiromasaoku 5:e02cd57242c7 116 IO.setGreenPower(1);
hiromasaoku 5:e02cd57242c7 117 exit(1);
hiromasaoku 17:dce982e0a383 118 }
hiromasaoku 17:dce982e0a383 119
hiromasaoku 4:e00e709d7173 120 int letternum;
hiromasaoku 4:e00e709d7173 121 fscanf(fp, "%d", &letternum);
hiromasaoku 5:e02cd57242c7 122 for(int i=0; i<letternum; i++) {
hiromasaoku 5:e02cd57242c7 123 letter bufl;
hiromasaoku 4:e00e709d7173 124 fscanf(fp, "%d", &bufl.pointnum);
hiromasaoku 5:e02cd57242c7 125 for(int j=0; j<bufl.pointnum; j++) {
hiromasaoku 12:d945fb6d4988 126 point2dl bufp;
hiromasaoku 4:e00e709d7173 127 fscanf(fp, "%d", &bufp.x);
hiromasaoku 4:e00e709d7173 128 fscanf(fp, "%d", &bufp.y);
hiromasaoku 9:4db47b8eb750 129 fscanf(fp, "%d", &bufp.laserSwitch);
hiromasaoku 4:e00e709d7173 130 bufl.letpoints.push_back(bufp);
hiromasaoku 4:e00e709d7173 131 }
hiromasaoku 16:6dd2e60bc5bc 132 alphabet.push_back(bufl);
hiromasaoku 4:e00e709d7173 133 }
hiromasaoku 5:e02cd57242c7 134
hiromasaoku 5:e02cd57242c7 135
mbedalvaro 0:4e12dea53fbe 136 // SETUP: --------------------------------------------------------------------------------------------
mbedalvaro 0:4e12dea53fbe 137 IO.init(); // note: serial speed can be changed by checking in the hardwareIO.cpp initialization
hiromasaoku 4:e00e709d7173 138
hiromasaoku 10:a3dd8ec4be60 139 // initialize the angle (arbitrary origin):
takapiasano 29:aa8fe9fccec8 140 //oldAngle=angle=0;
hiromasaoku 17:dce982e0a383 141
hiromasaoku 16:6dd2e60bc5bc 142 // Attach the external interrupt routine----------------------------------------------------------------------
takapiasano 29:aa8fe9fccec8 143 //superEncoder.attach_us(&encoderClock, 500);
hiromasaoku 20:8475768fc2f7 144 //clockEncoderPin.rise(&encoderClock);
hiromasaoku 20:8475768fc2f7 145 //clockEncoderPin.fall(&encoderClock);
hiromasaoku 17:dce982e0a383 146
hiromasaoku 16:6dd2e60bc5bc 147 //using renderclass.cpp ----------------------------------------------------------
hiromasaoku 19:8e9fe7d25b9c 148 //createTextPoints(textString);
hiromasaoku 16:6dd2e60bc5bc 149 myRender.setRender(&myText);
hiromasaoku 16:6dd2e60bc5bc 150 myRender.startRender();
hiromasaoku 17:dce982e0a383 151
hiromasaoku 10:a3dd8ec4be60 152 // Attach the periodic computing function:
takapiasano 29:aa8fe9fccec8 153 // We need to initialize the angleX and Y:
takapiasano 29:aa8fe9fccec8 154 angleX = inx;
takapiasano 29:aa8fe9fccec8 155 angleY = iny;
hiromasaoku 17:dce982e0a383 156 speedTimerCompute.attach_us(&computeSpeed, PERIODIC_COMPUTE);
hiromasaoku 17:dce982e0a383 157
hiromasaoku 16:6dd2e60bc5bc 158 // Set displaying laser powers--------------------------------------------------------------------------
takapiasano 29:aa8fe9fccec8 159 IO.setRGBPower(0);
hiromasaoku 9:4db47b8eb750 160 IO.setGreenPower(0);
mbedalvaro 0:4e12dea53fbe 161 wait_ms(100);
hiromasaoku 17:dce982e0a383 162
hiromasaoku 17:dce982e0a383 163
takapiasano 29:aa8fe9fccec8 164 //timer_v.start();
mbedalvaro 0:4e12dea53fbe 165 // MAIN LOOP: --------------------------------------------------------------------------------------------
mbedalvaro 0:4e12dea53fbe 166 while(1) {
hiromasaoku 17:dce982e0a383 167 if (pc.readable()>0) processSerial();
mbedalvaro 0:4e12dea53fbe 168 }
mbedalvaro 0:4e12dea53fbe 169 }
mbedalvaro 0:4e12dea53fbe 170
mbedalvaro 0:4e12dea53fbe 171 // --------------------------------------------------------------------------------------------
hiromasaoku 4:e00e709d7173 172 // String to store ALPHANUMERIC DATA (i.e., integers, floating point numbers, unsigned ints, etc represented as DEC) sent wirelessly:
hiromasaoku 17:dce982e0a383 173 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...
hiromasaoku 17:dce982e0a383 174 int indexStringData=0;//position of the byte in the string
mbedalvaro 0:4e12dea53fbe 175
hiromasaoku 17:dce982e0a383 176 void processSerial() {
hiromasaoku 17:dce982e0a383 177 start=true;
hiromasaoku 17:dce982e0a383 178 while(pc.readable()>0) {
hiromasaoku 4:e00e709d7173 179
hiromasaoku 17:dce982e0a383 180 char val =pc.getc();
hiromasaoku 4:e00e709d7173 181
hiromasaoku 17:dce982e0a383 182 // Save ASCII numeric characters (ASCII 0 - 9) on stringData:
hiromasaoku 17:dce982e0a383 183 if ((val >= '0') && (val <= '9')) { // this is 45 to 57 (included)
hiromasaoku 17:dce982e0a383 184 stringData[indexStringData] = val;
hiromasaoku 17:dce982e0a383 185 indexStringData++;
hiromasaoku 17:dce982e0a383 186 } else if ((val >= 'a') && (val <= 'z')) { // this is 45 to 57 (included)
hiromasaoku 19:8e9fe7d25b9c 187 myText.push_back(alphabet[val - 'a']);
hiromasaoku 17:dce982e0a383 188 //inputletters.push_back(val);
hiromasaoku 17:dce982e0a383 189 }
hiromasaoku 5:e02cd57242c7 190
hiromasaoku 19:8e9fe7d25b9c 191 else if (val == '/') {
hiromasaoku 19:8e9fe7d25b9c 192 myRender.stopRender();
hiromasaoku 19:8e9fe7d25b9c 193 myRender.setRender(&myText);
hiromasaoku 19:8e9fe7d25b9c 194 myRender.startRender();
hiromasaoku 19:8e9fe7d25b9c 195 } else if (val == '.') {
hiromasaoku 19:8e9fe7d25b9c 196 myRender.stopRender();
hiromasaoku 17:dce982e0a383 197 myText.clear();
hiromasaoku 19:8e9fe7d25b9c 198 myRender.setRender(&myText);
hiromasaoku 17:dce982e0a383 199 //inputletters.clear();
hiromasaoku 17:dce982e0a383 200 }
mbedalvaro 0:4e12dea53fbe 201
hiromasaoku 17:dce982e0a383 202 else if (val == 'X') {
hiromasaoku 17:dce982e0a383 203 beforeX = X;
hiromasaoku 17:dce982e0a383 204 stringData[indexStringData] = 0;
takapiasano 29:aa8fe9fccec8 205 potxx = atoi(stringData);
hiromasaoku 17:dce982e0a383 206 indexStringData=0;
takapiasano 29:aa8fe9fccec8 207 } else if (val == 'Y') {
hiromasaoku 17:dce982e0a383 208 beforeY = Y;
hiromasaoku 17:dce982e0a383 209 stringData[indexStringData] = 0;
takapiasano 29:aa8fe9fccec8 210 potyy = atoi(stringData);
hiromasaoku 17:dce982e0a383 211 indexStringData=0;
takapiasano 29:aa8fe9fccec8 212 } else if (val == 'R') {
hiromasaoku 17:dce982e0a383 213 stringData[indexStringData] = 0;
takapiasano 29:aa8fe9fccec8 214 myRender.laserColor = 2;
hiromasaoku 17:dce982e0a383 215 indexStringData=0;
hiromasaoku 17:dce982e0a383 216 //makeBuffer();
takapiasano 29:aa8fe9fccec8 217 } else if (val == 'G') {
hiromasaoku 17:dce982e0a383 218 stringData[indexStringData] = 0;
takapiasano 29:aa8fe9fccec8 219 myRender.laserColor = 3;
hiromasaoku 17:dce982e0a383 220 indexStringData=0;
hiromasaoku 17:dce982e0a383 221 //makeBuffer();
hiromasaoku 17:dce982e0a383 222 } else if (val == 'S') {
hiromasaoku 17:dce982e0a383 223 stringData[indexStringData] = 0;
hiromasaoku 20:8475768fc2f7 224 myRender.shearingSwitch = 1 - myRender.shearingSwitch;
hiromasaoku 20:8475768fc2f7 225 pc.printf("shearingSwitch : %d \n" , myRender.shearingSwitch);
hiromasaoku 17:dce982e0a383 226 indexStringData=0;
hiromasaoku 17:dce982e0a383 227 //makeBuffer();
takapiasano 29:aa8fe9fccec8 228 } else if (val == 'A') {
hiromasaoku 17:dce982e0a383 229 stringData[indexStringData] = 0;
takapiasano 29:aa8fe9fccec8 230 myRender.radious = atoi(stringData);
takapiasano 29:aa8fe9fccec8 231 pc.printf("st : %d \n" , angleIncrement);
takapiasano 29:aa8fe9fccec8 232 superEncoder.detach();
takapiasano 29:aa8fe9fccec8 233 superEncoder.attach_us(&encoderClock, attachSecond);
hiromasaoku 17:dce982e0a383 234 indexStringData=0;
hiromasaoku 17:dce982e0a383 235 //makeBuffer();
hiromasaoku 17:dce982e0a383 236 } else if (val == 'T') {
hiromasaoku 17:dce982e0a383 237 stringData[indexStringData] = 0;
hiromasaoku 17:dce982e0a383 238 attachSecond = atoi(stringData);
takapiasano 29:aa8fe9fccec8 239 encoderClock();
takapiasano 29:aa8fe9fccec8 240 myRender.startSwitch = 1;
takapiasano 29:aa8fe9fccec8 241 /*pc.printf("attachSecond : %d \n" , attachSecond);
takapiasano 29:aa8fe9fccec8 242
takapiasano 29:aa8fe9fccec8 243 superEncoder.detach();
takapiasano 29:aa8fe9fccec8 244 superEncoder.attach_us(&encoderClock, attachSecond);
hiromasaoku 17:dce982e0a383 245 indexStringData=0;
hiromasaoku 17:dce982e0a383 246 //makeBuffer();
takapiasano 29:aa8fe9fccec8 247 */
hiromasaoku 17:dce982e0a383 248 }
hiromasaoku 17:dce982e0a383 249 // X value?
hiromasaoku 17:dce982e0a383 250 /*else if (val=='x') {
hiromasaoku 17:dce982e0a383 251 stringData[indexStringData] = 0 ;
hiromasaoku 17:dce982e0a383 252 omegaX=atoi(stringData);
hiromasaoku 17:dce982e0a383 253 indexStringData=0;
hiromasaoku 17:dce982e0a383 254 //newPositionReady=true;
hiromasaoku 17:dce982e0a383 255 }
hiromasaoku 17:dce982e0a383 256 // Y value?
hiromasaoku 17:dce982e0a383 257 else if (val=='y') {
hiromasaoku 17:dce982e0a383 258 stringData[indexStringData] = 0 ;
hiromasaoku 17:dce982e0a383 259 omegaY=atoi(stringData);
hiromasaoku 17:dce982e0a383 260 indexStringData=0;
hiromasaoku 17:dce982e0a383 261 makeBuffer();
hiromasaoku 17:dce982e0a383 262 newPositionReady=true;
hiromasaoku 17:dce982e0a383 263 }
hiromasaoku 17:dce982e0a383 264
hiromasaoku 17:dce982e0a383 265 else if (val=='g') {
hiromasaoku 17:dce982e0a383 266 stringData[indexStringData] = 0 ;
hiromasaoku 17:dce982e0a383 267 int power=atoi(stringData);
hiromasaoku 17:dce982e0a383 268 indexStringData=0;
hiromasaoku 17:dce982e0a383 269 IO.setGreenPower(power);
hiromasaoku 17:dce982e0a383 270 } else if (val=='r') {
hiromasaoku 17:dce982e0a383 271 stringData[indexStringData] = 0 ;
hiromasaoku 17:dce982e0a383 272 int power=atoi(stringData);
hiromasaoku 17:dce982e0a383 273 indexStringData=0;
takapiasano 29:aa8fe9fccec8 274 IO.setRGBPower(power);
hiromasaoku 17:dce982e0a383 275 } else if (val=='c') {
hiromasaoku 17:dce982e0a383 276 stringData[indexStringData] = 0 ;
hiromasaoku 17:dce982e0a383 277 int power=atoi(stringData);
hiromasaoku 17:dce982e0a383 278 indexStringData=0;
hiromasaoku 17:dce982e0a383 279 IO.setRGBPower(power);
hiromasaoku 17:dce982e0a383 280 }
hiromasaoku 17:dce982e0a383 281 */
hiromasaoku 17:dce982e0a383 282
hiromasaoku 10:a3dd8ec4be60 283 }
hiromasaoku 17:dce982e0a383 284 }