main.cpp

00001 #include "PS2MS.h"
00002 #include "PS2MS_INIT.h"
00003 #include "mbed.h"
00004 #include "Servo.h"
00005 
00006 #define SENSORS_NUM 3
00007 #define BYTES_NUM 3
00008 
00009 #define DEBUG false
00010 
00011 #define MAX_OVERFLOWS 3
00012 
00013 #define MAX_REPLY_ERRORS 3
00014 
00015 Servo servoYaw(p21);
00016 Servo servoPitch(p24);
00017 
00018 Serial pc(USBTX, USBRX); // tx, rx
00019 /*
00020  * 0xFF: Reset command.
00021  * 0xF3: Set sample rate.
00022  * 0xF2: Read device type.
00023  * 0xE8: Set resolution.
00024  * 0xE6: Set scaling.
00025  * 0xF4: Enable device.
00026  */
00027 
00028 //TODO should Iuse sensor1_init? maybe no 255s?
00029 PS2MS* sensor[3];
00030 
00031 int process_sensor_input(int c, int bytenum, char* bytes, int ind);
00032 bool processACKReply(int ind);
00033 void sendError(int ind);
00034 bool getPacket(int ind);
00035 bool getMovementPacket(int ind);
00036 void sendResend(int ind);
00037 void sendError(int ind);
00038 int sendCommand(int ind, char command);
00039 void processSerial();
00040 
00041 int sensorXs[SENSORS_NUM];
00042 int sensorYs[SENSORS_NUM];
00043 bool sensorToPrint[SENSORS_NUM];
00044 
00045 char last_command[3];
00046 
00047 bool expectingAck1 = false, expectingAck2 = false, expectingAck3 = false;
00048 
00049 int replyErrors[3];
00050 
00051 static const int SERVO_YAW = 0;
00052 static const int SERVO_PITCH = 1;
00053 float servoPos[2];
00054 float servoAdj[2] = {0.097, 0.024}; // 0.097, 0.024
00055 bool mCommandCompleted[2] = {true, true};
00056 
00057 const float SERVO_SPEED_ADJ = 0.001;
00058 
00059 float servoSpeed = 1 * SERVO_SPEED_ADJ;
00060 
00061 Timer timer;
00062 
00063 int mDiscardedCount[3] = {0, 0, 0};
00064 const int MAX_DISCARDED = 3;
00065 
00066 PinName sensorPorts[3][2] = {
00067     {p18, p17}, // sensor 0
00068     {p15, p14}, // sensor 1
00069     {p26, p25} // sensor 2
00070 };
00071 
00072 void initializeSensor(int ind) {
00073     PS2MS_INIT s1(sensorPorts[ind][0], sensorPorts[ind][1]);
00074     printf("SENSOR_INIT % DONE on ports %d and %d\n\r", ind, sensorPorts[ind][0], sensorPorts[ind][1]);
00075     sensor[ind] = new PS2MS(sensorPorts[ind][0], sensorPorts[ind][1]);
00076 }
00077 
00078 bool notTooFarOff() {
00079     int maxAbs1 = abs(sensorXs[0]) > abs(sensorYs[0]) ? abs(sensorXs[0]) : abs(sensorYs[0]);
00080     int maxAbs2 = abs(sensorXs[1]) > abs(sensorYs[1]) ? abs(sensorXs[1]) : abs(sensorYs[1]);
00081     int maxAbs3 = abs(sensorXs[2]) > abs(sensorYs[2]) ? abs(sensorXs[2]) : abs(sensorYs[1]);
00082     
00083     if (maxAbs1 == 0) maxAbs1 = 1;
00084     if (maxAbs2 == 0) maxAbs2 = 1;
00085     if (maxAbs3 == 0) maxAbs3 = 1;
00086     
00087     if (maxAbs1 > maxAbs2 * 20) {
00088         return false;
00089     }
00090     
00091     if (maxAbs2 > maxAbs3 * 20) {
00092         return false;
00093     }
00094     
00095     if (maxAbs3 > maxAbs1 * 20) {
00096         return false;
00097     }
00098     
00099     return true;
00100 }
00101 
00102 //TODO switch to Tickers instead of checking these in a while loop, much cleaner
00103 long lastTickMs;
00104 long lastSendMs;
00105 const int TICK_EVERY_MS = 10;
00106 const int SEND_EVERY_MS = 30;
00107 
00108 int main()
00109 {
00110     printf("MAIN START\n\r");
00111     initializeSensor(0);
00112     initializeSensor(1);
00113     initializeSensor(2);
00114     //TODO: receive all pending packets here
00115 
00116     timer.start();
00117 
00118     float range = 0.00084;
00119     float position1 = 0.5;
00120     float position2 = 0.5;
00121 
00122     servoPos[SERVO_YAW] = position1 + servoAdj[SERVO_YAW];
00123     servoPos[SERVO_PITCH] = position2 + servoAdj[SERVO_PITCH];
00124 
00125     replyErrors[0] = replyErrors[1] = replyErrors[2] = 0;
00126 
00127     servoYaw.calibrate(range, 45.0);
00128     servoPitch.calibrate(range, 45.0);
00129     servoYaw = servoPos[SERVO_YAW];
00130     servoPitch = servoPos[SERVO_PITCH];
00131 
00132     sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
00133     int dir;
00134 
00135     bool awaitingPackets = false;
00136     
00137     lastTickMs = 0;
00138     lastSendMs = 0;
00139     
00140     int mTooFarOffFor = 0;
00141     const int MAX_TOO_FAR_OFF_COUNT = 2;
00142     
00143     long thisMs = 0, lastMs = 0;
00144     
00145     while(1) {
00146         if (pc.readable()) {
00147             processSerial();
00148         }
00149 
00150         if (abs(position1 - servoPos[SERVO_YAW]) > servoSpeed) {
00151             dir = position1 < servoPos[SERVO_YAW] ? 1 : -1;
00152             position1 += servoSpeed * dir;
00153             servoYaw = position1;
00154         } else {
00155             if (mCommandCompleted[SERVO_YAW] == false) {
00156                 printf("Command completed %d\n\r", timer.read_ms());
00157                 mCommandCompleted[SERVO_YAW] = true;
00158             }
00159             position1 = servoPos[SERVO_YAW];
00160         }
00161 
00162         if (abs(position2 - servoPos[SERVO_PITCH]) > servoSpeed) {
00163             dir = position2 < servoPos[SERVO_PITCH] ? 1 : -1;
00164             position2 += servoSpeed * dir;
00165             servoPitch = position2;
00166         } else {
00167             if (mCommandCompleted[SERVO_PITCH] == false) {
00168                 printf("Command completed %d\n\r", timer.read_ms());
00169                 mCommandCompleted[SERVO_PITCH] = true;
00170             }
00171             position2 = servoPos[SERVO_PITCH];
00172         }
00173         
00174         if (lastSendMs + SEND_EVERY_MS <= timer.read_ms()) {
00175             //printf("SEND %d\n\r", timer.read_ms());
00176             lastSendMs = timer.read_ms();
00177             int res;
00178             if (!awaitingPackets) {
00179                 //TODO: check for errors on send
00180                 res = sendCommand(0, '\xEB');
00181     
00182                 if (res) {
00183                     if (DEBUG) {
00184                         printf("%d: send error %d\n\r", 0, res);
00185                     }
00186                     res = sendCommand(0, '\xEB');
00187                     if (DEBUG) {
00188                         printf("%d: two failed sends %d\n\r", 0, res);
00189                     }
00190                 }
00191                 expectingAck1 = true;
00192                 res = sendCommand(1, '\xEB');
00193                 if (res) {
00194                     if (DEBUG) {
00195                         printf("%d: send error %d\n\r", 1, res);
00196                     }
00197                     res = sendCommand(1, '\xEB');
00198                     if (DEBUG) {
00199                         printf("%d: two failed sends %d\n\r", 1, res);
00200                     }
00201                 }
00202                 expectingAck2 = true;
00203                 res = sendCommand(2, '\xEB');
00204                 if (res) {
00205                     if (DEBUG) {
00206                         printf("%d: send error %d\n\r", 2, res);
00207                     }
00208                     res = sendCommand(2, '\xEB');
00209                     if (res) {
00210                         if (DEBUG) {
00211                             printf("%d: two failed sends %d\n\r", 2, res);
00212                         }
00213                     }
00214                 }
00215                 expectingAck3 = true;
00216                 awaitingPackets = true;
00217             }
00218         }
00219         // dilemma - else or no else?
00220         if (expectingAck1) {
00221             expectingAck1 = !getPacket(0);
00222         } 
00223         else if (expectingAck2) {
00224             expectingAck2 = !getPacket(1);
00225         }
00226         else if (expectingAck3) {
00227             expectingAck3 = !getPacket(2);
00228         }
00229         // TODO only prints when both are enabled now
00230         if (sensorToPrint[0] && sensorToPrint[1] && sensorToPrint[2]) {
00231             thisMs = timer.read_ms();
00232             if ((sensorXs[0] | sensorYs[0] | sensorXs[1] | sensorYs[1] | sensorXs[2] | sensorYs[2]) ) {
00233                 // some of the velocities are not 0
00234                 if (notTooFarOff()) {
00235                     printf("%d : %d %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
00236                        sensorXs[2], sensorYs[2], thisMs - lastMs);
00237                 }
00238                 else {
00239                     mTooFarOffFor++;
00240                     if (mTooFarOffFor < MAX_TOO_FAR_OFF_COUNT) {
00241                         printf("Too far off %d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
00242                            sensorXs[2], sensorYs[2]);
00243                         sendResend(0); expectingAck1 = true;
00244                         sendResend(1); expectingAck2 = true;
00245                         sendResend(2); expectingAck3 = true;
00246                         continue;
00247                     }
00248                     else {
00249                         printf("Ignoring a TooFarOff packet %d : %d %d %d %d %d %d\n\r", SENSORS_NUM, sensorXs[0], sensorYs[0], sensorXs[1], sensorYs[1],
00250                            sensorXs[2], sensorYs[2]);
00251                     }
00252                 }
00253             }
00254             lastMs = thisMs;
00255             mTooFarOffFor = 0;
00256             sensorToPrint[0] = sensorToPrint[1] = sensorToPrint[2] = false;
00257             sensorXs[0] = sensorYs[0] = sensorXs[1] = sensorYs[1] = sensorXs[2] = sensorYs[2] = 0;
00258             awaitingPackets = false;
00259         }
00260         while (lastTickMs + TICK_EVERY_MS > timer.read_ms()) {
00261             wait_ms(1);
00262         }
00263         lastTickMs = timer.read_ms();
00264         //printf("TICK %d\n\r", timer.read_ms());
00265     }
00266     /*
00267     TODO: Deallocate those somewhere
00268 
00269     delete[] sensor_init[0];
00270     delete[] sensor_init[1];
00271     delete[] sensor_init[2];
00272     delete[] sensor[0];
00273     delete[] sensor[1];
00274     delete[] sensor[2];
00275     */
00276 }
00277 
00278 #define MAX_ANGLE_STR_SIZE 100
00279 
00280 void processSerial()
00281 {
00282     char c = pc.getc();
00283     if (c <= '9' && c >= '0') {
00284         int servoNum = int(c - '0');
00285         if (servoNum == SERVO_YAW || servoNum == SERVO_PITCH) {
00286              char rotateAngleStr[MAX_ANGLE_STR_SIZE];
00287              pc.gets(rotateAngleStr, MAX_ANGLE_STR_SIZE);
00288              
00289              double rotateAngleNum = atoi(rotateAngleStr);
00290              if (rotateAngleNum > 90 || rotateAngleNum < -90) {
00291                  return;
00292              }
00293              rotateAngleNum = 0.5 + rotateAngleNum / 180.;
00294              servoPos[servoNum] = rotateAngleNum + servoAdj[servoNum];
00295              mCommandCompleted[servoNum] = false;
00296              printf("Command started %d\n\r", timer.read_ms());
00297         }
00298     }
00299     else if (c == 's') {
00300         char buffer[100];
00301         pc.gets(buffer, 100);
00302         int speedStep = atoi(buffer);
00303         servoSpeed = speedStep * SERVO_SPEED_ADJ;
00304         printf("Servo speed set to %d\n\r", speedStep);
00305     }
00306 }
00307 
00308 int sendCommand(int ind, char command)
00309 {
00310     int res;
00311     last_command[ind] = command;
00312     //res = sensor_init[ind]->send(command);
00313     res = sensor[ind]->sendCommand(command);
00314     //__enable_irq();
00315     if (res) {
00316         if (DEBUG) printf("error sending command %#x to %d\n\r", command, ind);
00317     }
00318     return res;
00319 }
00320 
00321 bool getPacket(int ind)
00322 {
00323     bool successful = processACKReply(ind);
00324     if (!successful) {
00325         sendResend(ind);
00326         successful = processACKReply(ind);
00327         if (!successful) {
00328             if (DEBUG) printf("DUNNO WHAT TO DO ACK Reply %d\n\r", ind);
00329             //wait(1);
00330             sendCommand(ind, '\xEB');
00331             return getPacket(ind);
00332         }
00333     }
00334     successful = getMovementPacket(ind);
00335 
00336     if (!successful) {
00337         printf("DISCARDING PACKET %d\n\r", ind);
00338         mDiscardedCount[ind]++;
00339         if (mDiscardedCount[ind] > MAX_DISCARDED) {
00340             initializeSensor(ind);
00341         }
00342         //if (DEBUG) {
00343         sendCommand(ind, '\xEB');
00344         //}
00345         return false;
00346     }
00347     mDiscardedCount[ind] = 0;
00348     return true;
00349 }
00350 
00351 void sendError(int ind)
00352 {
00353     sendCommand(ind, '\xFC');
00354 }
00355 
00356 void sendResend(int ind)
00357 {
00358     sendCommand(ind, '\xFE');
00359 }
00360 
00361 bool getMovementPacket(int ind)
00362 {
00363     char bytes[3];
00364     int c;
00365     for (int i = 0; i < 3; ++i) {
00366         c = sensor[ind]->getc();
00367         if (c < 0) {
00368             //printf("%d: 255\n\r", ind);
00369             return false;
00370         } else if (i == 0) {
00371             bytes[0] = c;
00372             if (!((c << 5) & 0x100)) {
00373                 // not byte[0] wrong offset, skip e
00374                 printf("%d: w %d ", ind, c);
00375                 c = sensor[ind]->getc();
00376                 while (c >= 0) {
00377                     printf("%d ", c);
00378                     c = sensor[ind]->getc();
00379                 }
00380                 printf("\n\r");
00381                 return false;
00382             }
00383         } else if (i == 1) {
00384             bytes[1] = c;
00385         } else if (i == 2) {
00386             bytes[2] = c;
00387             //printf("%d - %d %d %d\n\r", ind, bytes[0], bytes[1], bytes[2]);
00388 
00389             //TODO: check for overflow
00390             if ((1 << 6) & bytes[0]) {
00391                 //printf("%d: Overflow x %d %d %d!\n\r", ind, bytes[0], bytes[1], bytes[2]);
00392                 return false;
00393             } else if ((1 << 7) & bytes[0]) {
00394                 printf("%d: Overflow y %d %d %d!\n\r", ind, bytes[0], bytes[1], bytes[2]);
00395                 return false;
00396             }
00397             // check x and y signs
00398             else {
00399                 int x = bytes[1] - ((bytes[0] << 4) & 0x100);
00400                 int y = bytes[2] - ((bytes[0] << 3) & 0x100);
00401                 //printf("%s: x = %d   y = %d\n\r", id, x, y);
00402                 sensorXs[ind] = x;
00403                 sensorYs[ind] = y;
00404                 sensorToPrint[ind] = true;
00405                 //printf("%d        ", ind);
00406             }
00407         }
00408     }
00409     return true;
00410 }
00411 
00412 bool processACKReply(int ind)
00413 {
00414     int reply = sensor[ind]->getc();
00415     if (reply < 0) {
00416         if (DEBUG) printf("%d: Error %d", ind, reply);
00417         return false;
00418     } else if (reply == '\xFA') {
00419         //if (DEBUG) printf("%d: ACK ", ind);
00420         return true;
00421     } else if (reply == '\xEB') {
00422         if (DEBUG) printf("%d: READ_DATA ", ind);
00423         //TODO: inf loop possible here cause recursion
00424         return processACKReply(ind);
00425     } else if (reply == '\xFE') {
00426         if (last_command[ind] == '\xFE') {
00427             if (DEBUG) printf("%d: REPEATING_COMMAND BUT REPEAT_COMMAND!", ind, last_command[ind]);
00428             //wait(1);
00429             sendCommand(ind, '\xEB');
00430             return processACKReply(ind);
00431         }
00432         // repeat command
00433         if (DEBUG) printf("%d: REPEATING_COMMAND %#x", ind, last_command[ind]);
00434         //wait(1);
00435         sendCommand(ind, last_command[ind]);
00436         return processACKReply(ind);
00437     } else if (reply == '\xF2') {
00438         // get device ID??
00439         if (DEBUG) printf("%d: GET_DEVICE_ID %#x", ind, reply);
00440         //wait(1);
00441         return processACKReply(ind);
00442     } else {
00443         if (DEBUG) printf("%d: Unexpected ACK Reply %d\n\r", ind, reply);
00444         //wait(1);
00445         return processACKReply(ind);
00446     }
00447 }
00448 /*
00449 int process_sensor_input(int c, int bytenum, char* bytes, int ind)
00450 {
00451     if (c < 0) {
00452         //printf("%d: 255\n\r", ind);
00453         bytenum = -1;
00454     } else if (bytenum % BYTES_NUM == 0) {
00455         bytes[0] = c;
00456         if (!((c << 5) & 0x100)) {
00457             // not byte[0] wrong offset, skip c
00458             if (DEBUG) printf("%d: w %d\n\r", ind, c);
00459             bytenum = -1;
00460             sendError(ind);
00461         }
00462     } else if (bytenum % BYTES_NUM == 1) {
00463         bytes[1] = c;
00464     } else if (bytenum % BYTES_NUM == 2) {
00465         bytes[2] = c;
00466         //printf("%d - %d %d %d\n\r", ind, bytes[0], bytes[1], bytes[2]);
00467 
00468         //TODO: check for overflow
00469         if ((1 << 6) & bytes[0]) {
00470             printf("%d: Overflow x %d %d %d - %d!\n\r", ind, bytes[0], bytes[1], bytes[2], consecutiveOverflows[ind]);
00471             if (consecutiveOverflows[ind]++ < MAX_OVERFLOWS) {
00472                 sendError(ind);
00473             } else {
00474                 consecutiveOverflows[ind] = 0;
00475             }
00476             bytenum = -1;
00477         } else if ((1 << 7) & bytes[0]) {
00478             printf("%d: Overflow y %d %d %d - %d!\n\r", ind, bytes[0], bytes[1], bytes[2], consecutiveOverflows[ind]);
00479             if (consecutiveOverflows[ind]++ < MAX_OVERFLOWS) {
00480                 sendError(ind);
00481             } else {
00482                 consecutiveOverflows[ind] = 0;
00483             }
00484             bytenum = -1;
00485         }
00486         // check x and y signs
00487         else {
00488             int x = bytes[1] - ((bytes[0] << 4) & 0x100);
00489             int y = bytes[2] - ((bytes[0] << 3) & 0x100);
00490             //printf("%s: x = %d   y = %d\n\r", id, x, y);
00491             sensorXs[ind] = x;
00492             sensorYs[ind] = y;
00493             sensorToPrint[ind] = true;
00494             //printf("%d        ", ind);
00495             bytenum = -1;
00496         }
00497     }
00498     return (bytenum + 1) % BYTES_NUM;
00499 }
00500 */
00501 /*
00502 void sendError(int ind)
00503 {
00504     switch (ind) {
00505         case 0:
00506             sensor1_init.send('\xFE');
00507             expectingAck1 = true;
00508             break;
00509         case 1:
00510             sensor2_init.send('\xFE');
00511             expectingAck2 = true;
00512             break;
00513         case 2:
00514             sensor3_init.send('\xFE');
00515             expectingAck3 = true;
00516             break;
00517     }
00518 }*/