Takashi Asano / Mbed 2 deprecated linearMirrorMotion1017

sotsuron

Fork of linearMirrorMotion1017 by Hiromasa Oku

main.cpp@21:4095a071724d, 2013-06-07 (annotated)

Committer:
takapiasano
Date:
Fri Jun 07 05:01:40 2013 +0000
Revision:
21:4095a071724d
Parent:
20:8475768fc2f7
Child:
22:e86d63dfbbe1
demo horizontal

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;
hiromasaoku 10:a3dd8ec4be60 26 unsigned int dt=500, 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 21:4095a071724d 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-------------------------------------------------------------
hiromasaoku 10:a3dd8ec4be60 56 #define PERIODIC_COMPUTE 10000 // 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;
hiromasaoku 19:8e9fe7d25b9c 75 myRender.updateSpeed(angularSpeed,angle);
hiromasaoku 20:8475768fc2f7 76 void processSerial();
takapiasano 21:4095a071724d 77
hiromasaoku 10:a3dd8ec4be60 78 }
hiromasaoku 17:dce982e0a383 79
hiromasaoku 10:a3dd8ec4be60 80
hiromasaoku 17:dce982e0a383 81 int main()
hiromasaoku 17:dce982e0a383 82 {
hiromasaoku 10:a3dd8ec4be60 83 //read from TextFileLibrary ------------------------------------------------------
hiromasaoku 9:4db47b8eb750 84 FILE *fp = fopen("/local/text.txt", "r");
hiromasaoku 5:e02cd57242c7 85 if(!fp) {
hiromasaoku 5:e02cd57242c7 86 IO.setGreenPower(1);
hiromasaoku 5:e02cd57242c7 87 exit(1);
hiromasaoku 17:dce982e0a383 88 }
hiromasaoku 17:dce982e0a383 89
hiromasaoku 4:e00e709d7173 90 int letternum;
hiromasaoku 4:e00e709d7173 91 fscanf(fp, "%d", &letternum);
hiromasaoku 5:e02cd57242c7 92 for(int i=0; i<letternum; i++) {
hiromasaoku 5:e02cd57242c7 93 letter bufl;
hiromasaoku 4:e00e709d7173 94 fscanf(fp, "%d", &bufl.pointnum);
hiromasaoku 5:e02cd57242c7 95 for(int j=0; j<bufl.pointnum; j++) {
hiromasaoku 12:d945fb6d4988 96 point2dl bufp;
hiromasaoku 4:e00e709d7173 97 fscanf(fp, "%d", &bufp.x);
hiromasaoku 4:e00e709d7173 98 fscanf(fp, "%d", &bufp.y);
hiromasaoku 9:4db47b8eb750 99 fscanf(fp, "%d", &bufp.laserSwitch);
hiromasaoku 4:e00e709d7173 100 bufl.letpoints.push_back(bufp);
hiromasaoku 4:e00e709d7173 101 }
hiromasaoku 16:6dd2e60bc5bc 102 alphabet.push_back(bufl);
hiromasaoku 4:e00e709d7173 103 }
hiromasaoku 5:e02cd57242c7 104
hiromasaoku 5:e02cd57242c7 105
mbedalvaro 0:4e12dea53fbe 106 // SETUP: --------------------------------------------------------------------------------------------
mbedalvaro 0:4e12dea53fbe 107 IO.init(); // note: serial speed can be changed by checking in the hardwareIO.cpp initialization
hiromasaoku 4:e00e709d7173 108
hiromasaoku 10:a3dd8ec4be60 109 // initialize the angle (arbitrary origin):
hiromasaoku 10:a3dd8ec4be60 110 oldAngle=angle=0;
hiromasaoku 17:dce982e0a383 111
hiromasaoku 16:6dd2e60bc5bc 112 // Attach the external interrupt routine----------------------------------------------------------------------
takapiasano 21:4095a071724d 113 superEncoder.attach_us(&encoderClock, 500);
hiromasaoku 20:8475768fc2f7 114 //clockEncoderPin.rise(&encoderClock);
hiromasaoku 20:8475768fc2f7 115 //clockEncoderPin.fall(&encoderClock);
hiromasaoku 17:dce982e0a383 116
hiromasaoku 16:6dd2e60bc5bc 117 //using renderclass.cpp ----------------------------------------------------------
hiromasaoku 19:8e9fe7d25b9c 118 //createTextPoints(textString);
hiromasaoku 16:6dd2e60bc5bc 119 myRender.setRender(&myText);
hiromasaoku 16:6dd2e60bc5bc 120 myRender.startRender();
hiromasaoku 17:dce982e0a383 121
hiromasaoku 10:a3dd8ec4be60 122 // Attach the periodic computing function:
hiromasaoku 17:dce982e0a383 123 speedTimerCompute.attach_us(&computeSpeed, PERIODIC_COMPUTE);
hiromasaoku 17:dce982e0a383 124
hiromasaoku 16:6dd2e60bc5bc 125 // Set displaying laser powers--------------------------------------------------------------------------
mbedalvaro 0:4e12dea53fbe 126 IO.setRedPower(0);
hiromasaoku 9:4db47b8eb750 127 IO.setGreenPower(0);
mbedalvaro 0:4e12dea53fbe 128 wait_ms(100);
hiromasaoku 17:dce982e0a383 129
hiromasaoku 17:dce982e0a383 130
takapiasano 21:4095a071724d 131 //timer_v.start();
mbedalvaro 0:4e12dea53fbe 132 // MAIN LOOP: --------------------------------------------------------------------------------------------
mbedalvaro 0:4e12dea53fbe 133 while(1) {
hiromasaoku 17:dce982e0a383 134 if (pc.readable()>0) processSerial();
mbedalvaro 0:4e12dea53fbe 135 }
mbedalvaro 0:4e12dea53fbe 136 }
mbedalvaro 0:4e12dea53fbe 137
mbedalvaro 0:4e12dea53fbe 138 // --------------------------------------------------------------------------------------------
hiromasaoku 4:e00e709d7173 139 // String to store ALPHANUMERIC DATA (i.e., integers, floating point numbers, unsigned ints, etc represented as DEC) sent wirelessly:
hiromasaoku 17:dce982e0a383 140 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 141 int indexStringData=0;//position of the byte in the string
mbedalvaro 0:4e12dea53fbe 142
hiromasaoku 17:dce982e0a383 143 void processSerial() {
hiromasaoku 17:dce982e0a383 144 start=true;
hiromasaoku 17:dce982e0a383 145 while(pc.readable()>0) {
hiromasaoku 4:e00e709d7173 146
hiromasaoku 17:dce982e0a383 147 char val =pc.getc();
hiromasaoku 4:e00e709d7173 148
hiromasaoku 17:dce982e0a383 149 // Save ASCII numeric characters (ASCII 0 - 9) on stringData:
hiromasaoku 17:dce982e0a383 150 if ((val >= '0') && (val <= '9')) { // this is 45 to 57 (included)
hiromasaoku 17:dce982e0a383 151 stringData[indexStringData] = val;
hiromasaoku 17:dce982e0a383 152 indexStringData++;
hiromasaoku 17:dce982e0a383 153 } else if ((val >= 'a') && (val <= 'z')) { // this is 45 to 57 (included)
hiromasaoku 19:8e9fe7d25b9c 154 myText.push_back(alphabet[val - 'a']);
hiromasaoku 17:dce982e0a383 155 //inputletters.push_back(val);
hiromasaoku 17:dce982e0a383 156 }
hiromasaoku 5:e02cd57242c7 157
hiromasaoku 19:8e9fe7d25b9c 158 else if (val == '/') {
hiromasaoku 19:8e9fe7d25b9c 159 myRender.stopRender();
hiromasaoku 19:8e9fe7d25b9c 160 myRender.setRender(&myText);
hiromasaoku 19:8e9fe7d25b9c 161 myRender.startRender();
hiromasaoku 19:8e9fe7d25b9c 162 } else if (val == '.') {
hiromasaoku 19:8e9fe7d25b9c 163 myRender.stopRender();
hiromasaoku 17:dce982e0a383 164 myText.clear();
hiromasaoku 19:8e9fe7d25b9c 165 myRender.setRender(&myText);
hiromasaoku 17:dce982e0a383 166 //inputletters.clear();
hiromasaoku 17:dce982e0a383 167 }
mbedalvaro 0:4e12dea53fbe 168
hiromasaoku 17:dce982e0a383 169 else if (val == 'X') {
hiromasaoku 17:dce982e0a383 170 beforeX = X;
hiromasaoku 17:dce982e0a383 171 stringData[indexStringData] = 0;
hiromasaoku 17:dce982e0a383 172 X = atoi(stringData);
hiromasaoku 17:dce982e0a383 173 indexStringData=0;
takapiasano 21:4095a071724d 174 //vx = ((float)X-(float)beforeX) / (float)timer_v.read_us() *1000;
hiromasaoku 17:dce982e0a383 175 }
hiromasaoku 4:e00e709d7173 176
hiromasaoku 17:dce982e0a383 177 else if (val == 'Y') {
hiromasaoku 17:dce982e0a383 178 beforeY = Y;
hiromasaoku 17:dce982e0a383 179 stringData[indexStringData] = 0;
hiromasaoku 17:dce982e0a383 180 Y = atoi(stringData);
hiromasaoku 17:dce982e0a383 181 indexStringData=0;
hiromasaoku 17:dce982e0a383 182 //timer_v.stop();
hiromasaoku 17:dce982e0a383 183 //newSpeedReady = true;
hiromasaoku 17:dce982e0a383 184 //if( (Y-beforeY) > 5){
takapiasano 21:4095a071724d 185 //vy = ((float)Y-(float)beforeY) / (float)timer_v.read_us() *1000;
hiromasaoku 18:6f86abfae754 186
hiromasaoku 18:6f86abfae754 187 myRender.updateSpeed(vx, vy);
hiromasaoku 18:6f86abfae754 188
hiromasaoku 18:6f86abfae754 189
hiromasaoku 17:dce982e0a383 190 }
mbedalvaro 2:0548c7bf9fba 191
hiromasaoku 17:dce982e0a383 192 else if (val == 'D') {
hiromasaoku 17:dce982e0a383 193 stringData[indexStringData] = 0;
hiromasaoku 17:dce982e0a383 194 dt = atoi(stringData);
hiromasaoku 17:dce982e0a383 195 indexStringData=0;
hiromasaoku 17:dce982e0a383 196 //makeBuffer();
hiromasaoku 17:dce982e0a383 197 } else if (val == 'B') {
hiromasaoku 17:dce982e0a383 198 stringData[indexStringData] = 0;
hiromasaoku 17:dce982e0a383 199 dt_betWords = atoi(stringData);
hiromasaoku 17:dce982e0a383 200 indexStringData=0;
hiromasaoku 17:dce982e0a383 201 //makeBuffer();
hiromasaoku 17:dce982e0a383 202 } else if (val == 'S') {
hiromasaoku 17:dce982e0a383 203 stringData[indexStringData] = 0;
hiromasaoku 20:8475768fc2f7 204 myRender.shearingSwitch = 1 - myRender.shearingSwitch;
hiromasaoku 20:8475768fc2f7 205 pc.printf("shearingSwitch : %d \n" , myRender.shearingSwitch);
hiromasaoku 17:dce982e0a383 206 indexStringData=0;
hiromasaoku 17:dce982e0a383 207 //makeBuffer();
takapiasano 21:4095a071724d 208 } else if (val == 'A') {
hiromasaoku 17:dce982e0a383 209 stringData[indexStringData] = 0;
takapiasano 21:4095a071724d 210 angleIncrement = atoi(stringData);
takapiasano 21:4095a071724d 211 pc.printf("angleIncrement : %d \n" , angleIncrement);
takapiasano 21:4095a071724d 212 superEncoder.detach();
takapiasano 21:4095a071724d 213 superEncoder.attach_us(&encoderClock, attachSecond);
hiromasaoku 17:dce982e0a383 214 indexStringData=0;
hiromasaoku 17:dce982e0a383 215 //makeBuffer();
hiromasaoku 17:dce982e0a383 216 } else if (val == 'T') {
hiromasaoku 17:dce982e0a383 217 stringData[indexStringData] = 0;
hiromasaoku 17:dce982e0a383 218 attachSecond = atoi(stringData);
hiromasaoku 20:8475768fc2f7 219 pc.printf("attachSecond : %d \n" , attachSecond);
takapiasano 21:4095a071724d 220
takapiasano 21:4095a071724d 221 superEncoder.detach();
takapiasano 21:4095a071724d 222 superEncoder.attach_us(&encoderClock, attachSecond);
hiromasaoku 17:dce982e0a383 223 indexStringData=0;
hiromasaoku 17:dce982e0a383 224 //makeBuffer();
hiromasaoku 17:dce982e0a383 225 }
hiromasaoku 17:dce982e0a383 226 // X value?
hiromasaoku 17:dce982e0a383 227 /*else if (val=='x') {
hiromasaoku 17:dce982e0a383 228 stringData[indexStringData] = 0 ;
hiromasaoku 17:dce982e0a383 229 omegaX=atoi(stringData);
hiromasaoku 17:dce982e0a383 230 indexStringData=0;
hiromasaoku 17:dce982e0a383 231 //newPositionReady=true;
hiromasaoku 17:dce982e0a383 232 }
hiromasaoku 17:dce982e0a383 233 // Y value?
hiromasaoku 17:dce982e0a383 234 else if (val=='y') {
hiromasaoku 17:dce982e0a383 235 stringData[indexStringData] = 0 ;
hiromasaoku 17:dce982e0a383 236 omegaY=atoi(stringData);
hiromasaoku 17:dce982e0a383 237 indexStringData=0;
hiromasaoku 17:dce982e0a383 238 makeBuffer();
hiromasaoku 17:dce982e0a383 239 newPositionReady=true;
hiromasaoku 17:dce982e0a383 240 }
hiromasaoku 17:dce982e0a383 241
hiromasaoku 17:dce982e0a383 242 else if (val=='g') {
hiromasaoku 17:dce982e0a383 243 stringData[indexStringData] = 0 ;
hiromasaoku 17:dce982e0a383 244 int power=atoi(stringData);
hiromasaoku 17:dce982e0a383 245 indexStringData=0;
hiromasaoku 17:dce982e0a383 246 IO.setGreenPower(power);
hiromasaoku 17:dce982e0a383 247 } else if (val=='r') {
hiromasaoku 17:dce982e0a383 248 stringData[indexStringData] = 0 ;
hiromasaoku 17:dce982e0a383 249 int power=atoi(stringData);
hiromasaoku 17:dce982e0a383 250 indexStringData=0;
hiromasaoku 17:dce982e0a383 251 IO.setRedPower(power);
hiromasaoku 17:dce982e0a383 252 } else if (val=='c') {
hiromasaoku 17:dce982e0a383 253 stringData[indexStringData] = 0 ;
hiromasaoku 17:dce982e0a383 254 int power=atoi(stringData);
hiromasaoku 17:dce982e0a383 255 indexStringData=0;
hiromasaoku 17:dce982e0a383 256 IO.setRGBPower(power);
hiromasaoku 17:dce982e0a383 257 }
hiromasaoku 17:dce982e0a383 258 */
hiromasaoku 17:dce982e0a383 259
hiromasaoku 10:a3dd8ec4be60 260 }
hiromasaoku 17:dce982e0a383 261 }