Takashi Asano / Mbed 2 deprecated linearMirrorMotion1017

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of linearMirrorMotion1017 by Hiromasa Oku

main.cpp@26:74ecbf9985e1, 2013-07-22 (annotated)

Committer:
takapiasano
Date:
Mon Jul 22 08:09:18 2013 +0000
Revision:
26:74ecbf9985e1
Parent:
25:423050363215
Child:
27:38bc8dc67dab
ajust dekita

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