Julia DESMAZES / Mbed 2 deprecated Hexapode

Sorfware for Lexy ( Hexapode )

Dependencies:   mbed BLE_API X_NUCLEO_IDB0XA1 MODSERIAL

main.cpp@2:ca6d8d1f77d4, 2016-08-18 (annotated)

Committer:
Essenceia
Date:
Thu Aug 18 14:29:47 2016 +0000
Revision:
2:ca6d8d1f77d4
Parent:
1:8bab9152933e
Child:
3:13bd725bd47b
Startting to build main loop;

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 2:ca6d8d1f77d4 4 #include "LIB/Hexapod.h"
Essenceia 2:ca6d8d1f77d4 5
Essenceia 2:ca6d8d1f77d4 6 static Logger *Ordi = Logger::Instance();
Essenceia 1:8bab9152933e 7 int main()
Essenceia 1:8bab9152933e 8 {
Essenceia 2:ca6d8d1f77d4 9 //0.declaration des ressources
Essenceia 1:8bab9152933e 10
Essenceia 2:ca6d8d1f77d4 11 int ii, ij, ik, ix, iy, size, ind;
Essenceia 2:ca6d8d1f77d4 12 int cx, cy, attempts;
Essenceia 2:ca6d8d1f77d4 13 //serial ser;
Essenceia 2:ca6d8d1f77d4 14 packet *pack, *pack2, *pack_ask, *pack_data;
Essenceia 2:ca6d8d1f77d4 15 packet *pack_enable, *pack_disable;
Essenceia 2:ca6d8d1f77d4 16 hexapod hex;
Essenceia 2:ca6d8d1f77d4 17 data_chunk *d;
Essenceia 2:ca6d8d1f77d4 18 //logger log;
Essenceia 2:ca6d8d1f77d4 19 //SDL_Surface *screen;
Essenceia 2:ca6d8d1f77d4 20 //SDL_Event event;
Essenceia 2:ca6d8d1f77d4 21 //SDL_Joystick *joy;
Essenceia 2:ca6d8d1f77d4 22 int dsize, psize;
Essenceia 2:ca6d8d1f77d4 23 double time, lasttime, dt, lastdata, inittime;
Essenceia 2:ca6d8d1f77d4 24 uint8_t errcode;
Essenceia 2:ca6d8d1f77d4 25 float pos, avgtemp, joyval, maxval;
Essenceia 2:ca6d8d1f77d4 26 unsigned char chk;
Essenceia 2:ca6d8d1f77d4 27 bool cont, quit;
Essenceia 2:ca6d8d1f77d4 28 LowPowerTimer time;
Essenceia 2:ca6d8d1f77d4 29 //log.init("logfile", true);
Essenceia 2:ca6d8d1f77d4 30 time.start();
Essenceia 2:ca6d8d1f77d4 31 inittime = time.read();
Essenceia 2:ca6d8d1f77d4 32 /*
Essenceia 2:ca6d8d1f77d4 33 if (SDL_Init(SDL_INIT_VIDEO | SDL_INIT_JOYSTICK) < 0)
Essenceia 2:ca6d8d1f77d4 34 {
Essenceia 2:ca6d8d1f77d4 35 Ordi->log( "ERROR in SDL init: " << SDL_GetError() );
Essenceia 2:ca6d8d1f77d4 36 return -1;
Essenceia 2:ca6d8d1f77d4 37 }
Essenceia 2:ca6d8d1f77d4 38 atexit(SDL_Quit);
Essenceia 2:ca6d8d1f77d4 39 screen = SDL_SetVideoMode(800, 480, 32, SDL_NOFRAME | SDL_FULLSCREEN);
Essenceia 2:ca6d8d1f77d4 40 SDL_ShowCursor(0);
Essenceia 2:ca6d8d1f77d4 41 SDL_JoystickEventState(SDL_ENABLE);
Essenceia 2:ca6d8d1f77d4 42 // sort out joystick control
Essenceia 2:ca6d8d1f77d4 43 if (SDL_NumJoysticks()>0)
Essenceia 2:ca6d8d1f77d4 44 {
Essenceia 2:ca6d8d1f77d4 45 joy=SDL_JoystickOpen(0);
Essenceia 2:ca6d8d1f77d4 46 if (joy)
Essenceia 2:ca6d8d1f77d4 47 {
Essenceia 2:ca6d8d1f77d4 48 Ordi->log( "Joystick Connected." );
Essenceia 2:ca6d8d1f77d4 49 } else {
Essenceia 2:ca6d8d1f77d4 50 Ordi->log( "ERROR: could not connect to joystick!" );
Essenceia 2:ca6d8d1f77d4 51 return -1;
Essenceia 2:ca6d8d1f77d4 52 }
Essenceia 2:ca6d8d1f77d4 53 } else {
Essenceia 2:ca6d8d1f77d4 54 Ordi->log("ERROR: could not find joystick!" );
Essenceia 2:ca6d8d1f77d4 55 return -1;
Essenceia 2:ca6d8d1f77d4 56 }
Essenceia 2:ca6d8d1f77d4 57 */
Essenceia 2:ca6d8d1f77d4 58 // UDOO to Due is /dev/ttymxc3
Essenceia 2:ca6d8d1f77d4 59 ser.init_old("/dev/ttymxc3", false);
Essenceia 2:ca6d8d1f77d4 60
Essenceia 2:ca6d8d1f77d4 61 Ordi->log("Press Start to connect." );
Essenceia 2:ca6d8d1f77d4 62 cont = false;
Essenceia 2:ca6d8d1f77d4 63 while (!cont)
Essenceia 2:ca6d8d1f77d4 64 {
Essenceia 2:ca6d8d1f77d4 65 while (SDL_PollEvent(&event))
Essenceia 2:ca6d8d1f77d4 66 {
Essenceia 2:ca6d8d1f77d4 67 switch (event.type)
Essenceia 2:ca6d8d1f77d4 68 {
Essenceia 2:ca6d8d1f77d4 69 case SDL_JOYBUTTONDOWN:
Essenceia 2:ca6d8d1f77d4 70 if (event.jbutton.button == 7) cont = true;
Essenceia 2:ca6d8d1f77d4 71 break;
Essenceia 2:ca6d8d1f77d4 72 }
Essenceia 2:ca6d8d1f77d4 73 }
Essenceia 2:ca6d8d1f77d4 74 SDL_Delay(10);
Essenceia 2:ca6d8d1f77d4 75 }
Essenceia 2:ca6d8d1f77d4 76
Essenceia 2:ca6d8d1f77d4 77 // prep the motor enable / disable packets
Essenceia 2:ca6d8d1f77d4 78 pack_enable = new packet(18, 'A');
Essenceia 2:ca6d8d1f77d4 79 pack_enable->data[0] = 0x03;
Essenceia 2:ca6d8d1f77d4 80 pack_disable = new packet(18, 'A');
Essenceia 2:ca6d8d1f77d4 81 pack_disable->data[0] = 0x03;
Essenceia 2:ca6d8d1f77d4 82 for (ii=0; ii<18; ii++)
Essenceia 2:ca6d8d1f77d4 83 {
Essenceia 2:ca6d8d1f77d4 84 pack_enable->data[ii+1] = 0x01;
Essenceia 2:ca6d8d1f77d4 85 pack_disable->data[ii+1] = 0x00;
Essenceia 2:ca6d8d1f77d4 86 }
Essenceia 2:ca6d8d1f77d4 87
Essenceia 2:ca6d8d1f77d4 88 // before continuing, ask scontroller for servo data
Essenceia 2:ca6d8d1f77d4 89 // mostly to make sure it's ready to do stuff
Essenceia 2:ca6d8d1f77d4 90 Ordi->log("Confirming Connection.." );
Essenceia 2:ca6d8d1f77d4 91 // step 1: create a packet destined for the arbotix-m
Essenceia 2:ca6d8d1f77d4 92 pack = new packet(16, 'A'); // reasonable size?
Essenceia 2:ca6d8d1f77d4 93 // step 2: set command byte to 0x05, request for data
Essenceia 2:ca6d8d1f77d4 94 pack->data[0] = 0x05;
Essenceia 2:ca6d8d1f77d4 95 // step 3: set request byte to 0x01, requesting servo angles
Essenceia 2:ca6d8d1f77d4 96 pack->data[1] = 0x01;
Essenceia 2:ca6d8d1f77d4 97 // step 4: set return byte to 'D', send back to Due
Essenceia 2:ca6d8d1f77d4 98 pack->data[2] = 'U';
Essenceia 2:ca6d8d1f77d4 99 // step 5: send the packet
Essenceia 2:ca6d8d1f77d4 100 ser.send(pack, true);
Essenceia 2:ca6d8d1f77d4 101 sleep(1);
Essenceia 2:ca6d8d1f77d4 102
Essenceia 2:ca6d8d1f77d4 103 // the Arbotix-M might be off or initializing, so keep sending the request
Essenceia 2:ca6d8d1f77d4 104 // until you hear something back
Essenceia 2:ca6d8d1f77d4 105 pack2 = NULL;
Essenceia 2:ca6d8d1f77d4 106 while ((pack2 = ser.recv('U', false)) == NULL)
Essenceia 2:ca6d8d1f77d4 107 {
Essenceia 2:ca6d8d1f77d4 108 ser.send(pack, true);
Essenceia 2:ca6d8d1f77d4 109 sleep(1);
Essenceia 2:ca6d8d1f77d4 110 }
Essenceia 2:ca6d8d1f77d4 111 delete pack;
Essenceia 2:ca6d8d1f77d4 112
Essenceia 2:ca6d8d1f77d4 113 // we received data from the Arbotix-M, it's good to continue
Essenceia 2:ca6d8d1f77d4 114 Ordi->log( "System Ready." );
Essenceia 2:ca6d8d1f77d4 115 for (ii=0; ii<18; ii++)
Essenceia 2:ca6d8d1f77d4 116 {
Essenceia 2:ca6d8d1f77d4 117 memcpy(&pos, pack2->data+1+ii*(sizeof(float)+sizeof(uint8_t)),
Essenceia 2:ca6d8d1f77d4 118 sizeof(float));
Essenceia 2:ca6d8d1f77d4 119 Ordi->log( pos );
Essenceia 2:ca6d8d1f77d4 120 hex.servoangle[ii] = pos;
Essenceia 2:ca6d8d1f77d4 121 }
Essenceia 2:ca6d8d1f77d4 122 hex.setAngles();
Essenceia 2:ca6d8d1f77d4 123 delete pack2;
Essenceia 2:ca6d8d1f77d4 124
Essenceia 2:ca6d8d1f77d4 125 //rien qui ne nous interesse jusque ici
Essenceia 2:ca6d8d1f77d4 126 Ordi->log( "Angles read." );
Essenceia 2:ca6d8d1f77d4 127
Essenceia 2:ca6d8d1f77d4 128 // sit / stand
Essenceia 2:ca6d8d1f77d4 129 dsize = 100;
Essenceia 2:ca6d8d1f77d4 130 psize = SIZE;
Essenceia 2:ca6d8d1f77d4 131 pack = new packet(dsize, 'A', psize);
Essenceia 2:ca6d8d1f77d4 132 pack->data[0] = 0x01; // set servo positions
Essenceia 2:ca6d8d1f77d4 133
Essenceia 2:ca6d8d1f77d4 134 // max useable speed is 2.0 -> 1 foot per second
Essenceia 2:ca6d8d1f77d4 135 hex.speed = 0.0; // in cycles per second
Essenceia 2:ca6d8d1f77d4 136 hex.turning = 0.0; // [-1,1], rotation in z-axis
Essenceia 2:ca6d8d1f77d4 137
Essenceia 2:ca6d8d1f77d4 138 //ser.send(pack_enable);
Essenceia 2:ca6d8d1f77d4 139
Essenceia 2:ca6d8d1f77d4 140 // get ready to ask for data
Essenceia 2:ca6d8d1f77d4 141 pack_ask = new packet(16, 'A');
Essenceia 2:ca6d8d1f77d4 142 pack_ask->data[0] = 0x05;
Essenceia 2:ca6d8d1f77d4 143 pack_ask->data[1] = 0x02; // want temperature
Essenceia 2:ca6d8d1f77d4 144 pack_ask->data[2] = 'U';
Essenceia 2:ca6d8d1f77d4 145 pack_data = NULL;
Essenceia 2:ca6d8d1f77d4 146
Essenceia 2:ca6d8d1f77d4 147 Ordi->log( "Begin IK" );
Essenceia 2:ca6d8d1f77d4 148 // IK test
Essenceia 2:ca6d8d1f77d4 149 time = 0.0;
Essenceia 2:ca6d8d1f77d4 150 lasttime = time.read();
Essenceia 2:ca6d8d1f77d4 151 hex.safeStand();
Essenceia 2:ca6d8d1f77d4 152 while (hex.ssrunning)
Essenceia 2:ca6d8d1f77d4 153 {
Essenceia 2:ca6d8d1f77d4 154 dt = (getTime() - lasttime);
Essenceia 2:ca6d8d1f77d4 155 lasttime = getTime();
Essenceia 2:ca6d8d1f77d4 156 hex.step(dt);
Essenceia 2:ca6d8d1f77d4 157 // package positions
Essenceia 2:ca6d8d1f77d4 158 for (ii=0; ii<18; ii++)
Essenceia 2:ca6d8d1f77d4 159 {
Essenceia 2:ca6d8d1f77d4 160 pos = hex.servoangle[ii];
Essenceia 2:ca6d8d1f77d4 161 memcpy(pack->data+1+ii*sizeof(float),
Essenceia 2:ca6d8d1f77d4 162 &pos, sizeof(float));
Essenceia 2:ca6d8d1f77d4 163 }
Essenceia 2:ca6d8d1f77d4 164 ser.send(pack);
Essenceia 2:ca6d8d1f77d4 165 usleep(20*1000);
Essenceia 2:ca6d8d1f77d4 166
Essenceia 2:ca6d8d1f77d4 167 }
Essenceia 2:ca6d8d1f77d4 168 lasttime = getTime();
Essenceia 2:ca6d8d1f77d4 169 lastdata = lasttime;
Essenceia 2:ca6d8d1f77d4 170 ser.send(pack_ask);
Essenceia 2:ca6d8d1f77d4 171 usleep(20*1000);
Essenceia 2:ca6d8d1f77d4 172
Essenceia 2:ca6d8d1f77d4 173 quit = false;
Essenceia 2:ca6d8d1f77d4 174 while (!quit)
Essenceia 2:ca6d8d1f77d4 175 {
Essenceia 2:ca6d8d1f77d4 176 dt = (getTime() - lasttime);
Essenceia 2:ca6d8d1f77d4 177 time += dt;
Essenceia 2:ca6d8d1f77d4 178 lasttime = getTime();
Essenceia 2:ca6d8d1f77d4 179 hex.step(dt);
Essenceia 2:ca6d8d1f77d4 180 d = new data_chunk('P', inittime);
Essenceia 2:ca6d8d1f77d4 181 d->add(hex.dr_ang);
Essenceia 2:ca6d8d1f77d4 182 d->add(hex.dr_xpos);
Essenceia 2:ca6d8d1f77d4 183 d->add(hex.dr_ypos);
Essenceia 2:ca6d8d1f77d4 184 d->add(hex.maxsweep);
Essenceia 2:ca6d8d1f77d4 185 d->add(hex.speedmodifier);
Essenceia 2:ca6d8d1f77d4 186 log.send(d);
Essenceia 2:ca6d8d1f77d4 187 // package positions
Essenceia 2:ca6d8d1f77d4 188 for (ii=0; ii<18; ii++)
Essenceia 2:ca6d8d1f77d4 189 {
Essenceia 2:ca6d8d1f77d4 190 pos = hex.servoangle[ii];
Essenceia 2:ca6d8d1f77d4 191 memcpy(pack->data+1+(ii)*sizeof(float),
Essenceia 2:ca6d8d1f77d4 192 &pos, sizeof(float));
Essenceia 2:ca6d8d1f77d4 193 }
Essenceia 2:ca6d8d1f77d4 194 ser.send(pack);
Essenceia 2:ca6d8d1f77d4 195 // ask for data?
Essenceia 2:ca6d8d1f77d4 196
Essenceia 2:ca6d8d1f77d4 197 if (getTime() - lastdata > 1.0)
Essenceia 2:ca6d8d1f77d4 198 {
Essenceia 2:ca6d8d1f77d4 199 if ((pack2=ser.recv('U',false)) != NULL)
Essenceia 2:ca6d8d1f77d4 200 {
Essenceia 2:ca6d8d1f77d4 201 maxval = 1e-10;
Essenceia 2:ca6d8d1f77d4 202 avgtemp = 0.0;
Essenceia 2:ca6d8d1f77d4 203 for (ii=0; ii<18; ii++)
Essenceia 2:ca6d8d1f77d4 204 {
Essenceia 2:ca6d8d1f77d4 205 memcpy(&pos, pack2->data+1+ii*(sizeof(float)+sizeof(uint8_t)),
Essenceia 2:ca6d8d1f77d4 206 sizeof(float));
Essenceia 2:ca6d8d1f77d4 207 memcpy(&errcode, pack2->data+1+ii*(sizeof(float)+sizeof(uint8_t))+sizeof(float), sizeof(uint8_t));
Essenceia 2:ca6d8d1f77d4 208 avgtemp += pos;
Essenceia 2:ca6d8d1f77d4 209 if (pos > maxval) maxval = pos;
Essenceia 2:ca6d8d1f77d4 210 if (errcode != 0) // 32=overload, 4=temperature, 1=voltage
Essenceia 2:ca6d8d1f77d4 211 Ordi->log( "SERVO ERROR: "<< (int)(errcode) << " ON SERVO " << ii );
Essenceia 2:ca6d8d1f77d4 212 }
Essenceia 2:ca6d8d1f77d4 213 avgtemp /= 18.;
Essenceia 2:ca6d8d1f77d4 214 d = new data_chunk('T', inittime);
Essenceia 2:ca6d8d1f77d4 215 d->add(avgtemp);
Essenceia 2:ca6d8d1f77d4 216 log.send(d);
Essenceia 2:ca6d8d1f77d4 217 delete pack2;
Essenceia 2:ca6d8d1f77d4 218 lastdata = getTime();
Essenceia 2:ca6d8d1f77d4 219 pack2 = NULL;
Essenceia 2:ca6d8d1f77d4 220 ser.send(pack_ask);
Essenceia 2:ca6d8d1f77d4 221 usleep(20*1000);
Essenceia 2:ca6d8d1f77d4 222 }
Essenceia 2:ca6d8d1f77d4 223 }
Essenceia 2:ca6d8d1f77d4 224
Essenceia 2:ca6d8d1f77d4 225 while (SDL_PollEvent(&event))
Essenceia 2:ca6d8d1f77d4 226 {
Essenceia 2:ca6d8d1f77d4 227 switch (event.type)
Essenceia 2:ca6d8d1f77d4 228 {
Essenceia 2:ca6d8d1f77d4 229 case SDL_JOYBUTTONDOWN:
Essenceia 2:ca6d8d1f77d4 230 if (event.jbutton.button == 7) quit = true;
Essenceia 2:ca6d8d1f77d4 231 break;
Essenceia 2:ca6d8d1f77d4 232 case SDL_JOYAXISMOTION:
Essenceia 2:ca6d8d1f77d4 233 joyval = -event.jaxis.value/32767.;
Essenceia 2:ca6d8d1f77d4 234 if (event.jaxis.axis == 1) // L stick, yaxis
Essenceia 2:ca6d8d1f77d4 235 {
Essenceia 2:ca6d8d1f77d4 236 if (joyval > 0.1) hex.speed = 0.5*(joyval - 0.1);
Essenceia 2:ca6d8d1f77d4 237 else if (joyval < -0.1) hex.speed = 0.5*(joyval + 0.1);
Essenceia 2:ca6d8d1f77d4 238 else hex.speed = 0.0;
Essenceia 2:ca6d8d1f77d4 239 }
Essenceia 2:ca6d8d1f77d4 240 if (event.jaxis.axis == 2) // R stick, xaxis
Essenceia 2:ca6d8d1f77d4 241 {
Essenceia 2:ca6d8d1f77d4 242 if (joyval > 0.1) hex.turning = (joyval-0.1);
Essenceia 2:ca6d8d1f77d4 243 else if (joyval < -0.1) hex.turning = (joyval+0.1);
Essenceia 2:ca6d8d1f77d4 244 else hex.turning = 0.0;
Essenceia 2:ca6d8d1f77d4 245 }
Essenceia 2:ca6d8d1f77d4 246 if (event.jaxis.axis == 3) // R stick, yaxis
Essenceia 2:ca6d8d1f77d4 247 {
Essenceia 2:ca6d8d1f77d4 248 if (joyval > 0.1) hex.standheight = (joyval-0.1)*2.0;
Essenceia 2:ca6d8d1f77d4 249 else if (joyval < -0.1) hex.standheight = (joyval+0.1)*2.0;
Essenceia 2:ca6d8d1f77d4 250 else hex.standheight = 0.0;
Essenceia 2:ca6d8d1f77d4 251 }
Essenceia 2:ca6d8d1f77d4 252 break;
Essenceia 2:ca6d8d1f77d4 253 }
Essenceia 2:ca6d8d1f77d4 254 }
Essenceia 2:ca6d8d1f77d4 255 SDL_Delay(20);
Essenceia 2:ca6d8d1f77d4 256 }
Essenceia 2:ca6d8d1f77d4 257
Essenceia 2:ca6d8d1f77d4 258 Ordi->log( "Quitting.." );
Essenceia 2:ca6d8d1f77d4 259 ser.send(pack_disable);
Essenceia 2:ca6d8d1f77d4 260 delete pack;
Essenceia 2:ca6d8d1f77d4 261 log.close();
Essenceia 2:ca6d8d1f77d4 262 ser.close();
Essenceia 1:8bab9152933e 263 return 0;
Essenceia 1:8bab9152933e 264 }
Essenceia 1:8bab9152933e 265