Takashi Asano / Mbed 2 deprecated linearMirrorMotion1017

sotsuron

Fork of linearMirrorMotion1017 by Hiromasa Oku

main.cpp@16:6dd2e60bc5bc, 2013-05-20 (annotated)

Committer:
hiromasaoku
Date:
Mon May 20 08:25:22 2013 +0000
Revision:
16:6dd2e60bc5bc
Parent:
15:1d931a305464
Child:
17:dce982e0a383
a safety commit

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