Julia DESMAZES / Mbed 2 deprecated Hexapode

Sorfware for Lexy ( Hexapode )

Dependencies:   mbed BLE_API X_NUCLEO_IDB0XA1 MODSERIAL

main.cpp@5:58acbceb4f9e, 2016-08-20 (annotated)

Committer:
Essenceia
Date:
Sat Aug 20 19:21:54 2016 +0000
Revision:
5:58acbceb4f9e
Parent:
4:49b5cde0aecc 
Now needs to be debugger;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Essenceia 1:8bab9152933e 1 #include "mbed.h"
Essenceia 1:8bab9152933e 2 #include "COM/SSC-32.h"
Essenceia 2:ca6d8d1f77d4 3 #include "COM/LOGGER.h"
Essenceia 4:49b5cde0aecc 4 #include "COM/Event.h"
Essenceia 2:ca6d8d1f77d4 5 #include "LIB/Hexapod.h"
Essenceia 2:ca6d8d1f77d4 6
Essenceia 2:ca6d8d1f77d4 7 static Logger *Ordi = Logger::Instance();
Essenceia 4:49b5cde0aecc 8 static event *evnt = event::Instance();
Essenceia 1:8bab9152933e 9 int main()
Essenceia 1:8bab9152933e 10 {
Essenceia 4:49b5cde0aecc 11 //0.declaration des ressources
Essenceia 1:8bab9152933e 12
Essenceia 2:ca6d8d1f77d4 13 int ii, ij, ik, ix, iy, size, ind;
Essenceia 2:ca6d8d1f77d4 14 int cx, cy, attempts;
Essenceia 2:ca6d8d1f77d4 15 //serial ser;
Essenceia 2:ca6d8d1f77d4 16 hexapod hex;
Essenceia 2:ca6d8d1f77d4 17 //logger log;
Essenceia 2:ca6d8d1f77d4 18 //SDL_Surface *screen;
Essenceia 2:ca6d8d1f77d4 19 //SDL_Event event;
Essenceia 2:ca6d8d1f77d4 20 //SDL_Joystick *joy;
Essenceia 2:ca6d8d1f77d4 21 int dsize, psize;
Essenceia 2:ca6d8d1f77d4 22 double time, lasttime, dt, lastdata, inittime;
Essenceia 2:ca6d8d1f77d4 23 uint8_t errcode;
Essenceia 2:ca6d8d1f77d4 24 float pos, avgtemp, joyval, maxval;
Essenceia 2:ca6d8d1f77d4 25 unsigned char chk;
Essenceia 2:ca6d8d1f77d4 26 bool cont, quit;
Essenceia 3:13bd725bd47b 27 //My adds
Essenceia 2:ca6d8d1f77d4 28 LowPowerTimer time;
Essenceia 3:13bd725bd47b 29 SSC_32 ser;
Essenceia 2:ca6d8d1f77d4 30 //log.init("logfile", true);
Essenceia 2:ca6d8d1f77d4 31 time.start();
Essenceia 2:ca6d8d1f77d4 32 inittime = time.read();
Essenceia 2:ca6d8d1f77d4 33
Essenceia 2:ca6d8d1f77d4 34 // max useable speed is 2.0 -> 1 foot per second
Essenceia 2:ca6d8d1f77d4 35 hex.speed = 0.0; // in cycles per second
Essenceia 2:ca6d8d1f77d4 36 hex.turning = 0.0; // [-1,1], rotation in z-axis
Essenceia 2:ca6d8d1f77d4 37
Essenceia 2:ca6d8d1f77d4 38 Ordi->log( "Begin IK" );
Essenceia 2:ca6d8d1f77d4 39 // IK test
Essenceia 2:ca6d8d1f77d4 40 time = 0.0;
Essenceia 2:ca6d8d1f77d4 41 lasttime = time.read();
Essenceia 2:ca6d8d1f77d4 42 hex.safeStand();
Essenceia 4:49b5cde0aecc 43 while (hex.ssrunning) {
Essenceia 3:13bd725bd47b 44 dt = (time.read() - lasttime);
Essenceia 3:13bd725bd47b 45 lasttime = time.read();
Essenceia 2:ca6d8d1f77d4 46 hex.step(dt);
Essenceia 2:ca6d8d1f77d4 47 // package positions
Essenceia 4:49b5cde0aecc 48 for (ii=0; ii<18; ii++) {
Essenceia 3:13bd725bd47b 49 ser.add(ii,hex.servoangle[ii]);
Essenceia 4:49b5cde0aecc 50
Essenceia 2:ca6d8d1f77d4 51 }
Essenceia 3:13bd725bd47b 52 ser.send();
Essenceia 3:13bd725bd47b 53 wait_us(20*1000);
Essenceia 2:ca6d8d1f77d4 54
Essenceia 2:ca6d8d1f77d4 55 }
Essenceia 3:13bd725bd47b 56 lasttime = time.read();
Essenceia 2:ca6d8d1f77d4 57 lastdata = lasttime;
Essenceia 3:13bd725bd47b 58 ser.send();
Essenceia 3:13bd725bd47b 59 wait_us(20*1000);
Essenceia 2:ca6d8d1f77d4 60
Essenceia 2:ca6d8d1f77d4 61 quit = false;
Essenceia 4:49b5cde0aecc 62 while (!quit) {
Essenceia 3:13bd725bd47b 63 dt = (time.read() - lasttime);
Essenceia 2:ca6d8d1f77d4 64 time += dt;
Essenceia 3:13bd725bd47b 65 lasttime = time.read();
Essenceia 2:ca6d8d1f77d4 66 hex.step(dt);
Essenceia 2:ca6d8d1f77d4 67 // package positions
Essenceia 4:49b5cde0aecc 68 for (ii=0; ii<18; ii++) {
Essenceia 3:13bd725bd47b 69 ser.add(ii, hex.servoangle[ii]);
Essenceia 2:ca6d8d1f77d4 70 }
Essenceia 3:13bd725bd47b 71 ser.send();
Essenceia 4:49b5cde0aecc 72 // ask for data?
Essenceia 4:49b5cde0aecc 73
Essenceia 4:49b5cde0aecc 74 while (evnt->has_new_event()) {
Essenceia 4:49b5cde0aecc 75
Essenceia 4:49b5cde0aecc 76 if (evnt->has_button_presed()) {
Essenceia 4:49b5cde0aecc 77 if (evnt->get_val_B()=='7') quit = true;
Essenceia 4:49b5cde0aecc 78 }
Essenceia 4:49b5cde0aecc 79 if (evnt->has_J_pressed()) { // J stick, yaxis
Essenceia 4:49b5cde0aecc 80 joyval = evnt->get_val_J();
Essenceia 4:49b5cde0aecc 81 if (joyval > 0.1) hex.speed = 0.5*(joyval - 0.1);
Essenceia 4:49b5cde0aecc 82 else if (joyval < -0.1) hex.speed = 0.5*(joyval + 0.1);
Essenceia 4:49b5cde0aecc 83 else hex.speed = 0.0;
Essenceia 2:ca6d8d1f77d4 84 }
Essenceia 4:49b5cde0aecc 85 if (evnt->has_T_pressed()) { // T stick, xaxis
Essenceia 4:49b5cde0aecc 86 joyval = evnt->get_val_T();
Essenceia 4:49b5cde0aecc 87 if (joyval > 0.1) hex.turning = (joyval-0.1);
Essenceia 4:49b5cde0aecc 88 else if (joyval < -0.1) hex.turning = (joyval+0.1);
Essenceia 4:49b5cde0aecc 89 else hex.turning = 0.0;
Essenceia 4:49b5cde0aecc 90 }
Essenceia 4:49b5cde0aecc 91 if (evnt->has_R_pressed()) { // R stick, yaxis
Essenceia 4:49b5cde0aecc 92 joyval = evnt->get_val_R();
Essenceia 4:49b5cde0aecc 93 if (joyval > 0.1) hex.standheight = (joyval-0.1)*2.0;
Essenceia 4:49b5cde0aecc 94 else if (joyval < -0.1) hex.standheight = (joyval+0.1)*2.0;
Essenceia 4:49b5cde0aecc 95 else hex.standheight = 0.0;
Essenceia 4:49b5cde0aecc 96 }
Essenceia 4:49b5cde0aecc 97
Essenceia 2:ca6d8d1f77d4 98 }
Essenceia 4:49b5cde0aecc 99 wait_us(20);
Essenceia 2:ca6d8d1f77d4 100 }
Essenceia 2:ca6d8d1f77d4 101
Essenceia 2:ca6d8d1f77d4 102 Ordi->log( "Quitting.." );
Essenceia 2:ca6d8d1f77d4 103 ser.close();
Essenceia 1:8bab9152933e 104 return 0;
Essenceia 4:49b5cde0aecc 105 }