Hiromasa Oku / Mbed 2 deprecated linearMirrorMotion1017

this version 10/17

Dependencies:   mbed

Fork of linearMirrorMotion by Alvaro Cassinelli

main.cpp@17:dce982e0a383, 2013-05-20 (annotated)

Committer:
hiromasaoku
Date:
Mon May 20 08:51:07 2013 +0000
Revision:
17:dce982e0a383
Parent:
16:6dd2e60bc5bc
Child:
18:6f86abfae754
error vanished!!

Who changed what in which revision?

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