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@25:423050363215, 2013-07-03 (annotated)

Committer:
takapiasano
Date:
Wed Jul 03 07:43:48 2013 +0000
Revision:
25:423050363215
Parent:
22:e86d63dfbbe1
Child:
26:74ecbf9985e1
Tenmetsu for Saccade mirror

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