main.cpp

00001 #include "mbed.h"
00002 #include "COM/SSC-32.h"
00003 #include "COM/LOGGER.h"
00004 #include "COM/Event.h"
00005 #include "LIB/Hexapod.h"
00006 
00007 static Logger *Ordi = Logger::Instance();
00008 static event *evnt = event::Instance();
00009 int main()
00010 {
00011     //0.declaration des ressources
00012 
00013     int ii, ij, ik, ix, iy, size, ind;
00014     int cx, cy, attempts;
00015     //serial ser;
00016     hexapod hex;
00017     //logger log;
00018     //SDL_Surface *screen;
00019     //SDL_Event event;
00020     //SDL_Joystick *joy;
00021     int dsize, psize;
00022     double time, lasttime, dt, lastdata, inittime;
00023     uint8_t errcode;
00024     float pos, avgtemp, joyval, maxval;
00025     unsigned char chk;
00026     bool cont, quit;
00027     //My adds
00028     LowPowerTimer time;
00029     SSC_32 ser;
00030     //log.init("logfile", true);
00031     time.start();
00032     inittime = time.read();
00033 
00034     // max useable speed is 2.0 -> 1 foot per second
00035     hex.speed = 0.0; // in cycles per second
00036     hex.turning = 0.0; // [-1,1], rotation in z-axis
00037 
00038     Ordi->log( "Begin IK" );
00039     // IK test
00040     time = 0.0;
00041     lasttime = time.read();
00042     hex.safeStand();
00043     while (hex.ssrunning) {
00044         dt = (time.read() - lasttime);
00045         lasttime = time.read();
00046         hex.step(dt);
00047         // package positions
00048         for (ii=0; ii<18; ii++) {
00049             ser.add(ii,hex.servoangle[ii]);
00050 
00051         }
00052         ser.send();
00053         wait_us(20*1000);
00054 
00055     }
00056     lasttime = time.read();
00057     lastdata = lasttime;
00058     ser.send();
00059     wait_us(20*1000);
00060 
00061     quit = false;
00062     while (!quit) {
00063         dt = (time.read() - lasttime);
00064         time += dt;
00065         lasttime = time.read();
00066         hex.step(dt);
00067         // package positions
00068         for (ii=0; ii<18; ii++) {
00069             ser.add(ii, hex.servoangle[ii]);
00070         }
00071         ser.send();
00072         // ask for data?
00073 
00074         while (evnt->has_new_event()) {
00075 
00076             if (evnt->has_button_presed()) {
00077                 if (evnt->get_val_B()=='7') quit = true;
00078             }
00079             if (evnt->has_J_pressed()) { // J stick, yaxis
00080                 joyval = evnt->get_val_J();
00081                 if (joyval > 0.1) hex.speed = 0.5*(joyval - 0.1);
00082                 else if (joyval < -0.1) hex.speed = 0.5*(joyval + 0.1);
00083                 else hex.speed = 0.0;
00084             }
00085             if (evnt->has_T_pressed()) { // T stick, xaxis
00086                 joyval = evnt->get_val_T();
00087                 if (joyval > 0.1) hex.turning = (joyval-0.1);
00088                 else if (joyval < -0.1) hex.turning = (joyval+0.1);
00089                 else hex.turning = 0.0;
00090             }
00091             if (evnt->has_R_pressed()) { // R stick, yaxis
00092                 joyval = evnt->get_val_R();
00093                 if (joyval > 0.1) hex.standheight = (joyval-0.1)*2.0;
00094                 else if (joyval < -0.1) hex.standheight = (joyval+0.1)*2.0;
00095                 else hex.standheight = 0.0;
00096             }
00097 
00098         }
00099         wait_us(20);
00100     }
00101 
00102     Ordi->log( "Quitting.." );
00103     ser.close();
00104     return 0;
00105 }