Hexapod.h

00001 #ifndef HEXAPOD_H
00002 #define HEXAPOD_H
00003 #include <math.h>
00004 #include "Bezier.h"
00005 #ifndef PI
00006 #define PI 3.14159265
00007 #endif
00008 #define DEGTORAD 0.01745329251
00009 #define RADTODEG 57.2957795131
00010 #define MAX_SPEED 1.5
00011 #define SS_DURATION 2.0
00012 #define TURN_TOL 0.005
00013 #define MAXITER 300
00014 // position tolerance in cm
00015 #define TOLERANCE 0.1
00016 // angle to move things away from bounds by, in radians
00017 #define ANGEPS 0.01
00018 
00019 // in turning value per second
00020 #define TURN_SLEW 0.8
00021 #define SPEED_SLEW 0.4
00022 
00023 using namespace std;
00024 
00025 /* servo IDs:
00026  *           back
00027  *  06 10 08 ---- 07 09 11 
00028  *  12 16 14 ---- 13 15 17
00029  *  18 04 02 ---- 01 03 05
00030  *           front
00031  *
00032  * convert to index:
00033  *  (LEG 5) 17 16 15 ---- 06 07 08 (LEG 2)
00034  *  (LEG 4) 14 13 12 ---- 03 04 05 (LEG 1)
00035  *  (LEG 3) 11 10 09 ---- 00 01 02 (LEG 0)
00036  *
00037  * tibia, femur, coxa ---- coxa, femur, tibia
00038  *
00039  * limits (ignoring intersection with other limbs):
00040  *  coxa: 245 - 75
00041  *  femur: 250 - 55
00042  *  tibia: 215 - 40
00043  */
00044 
00045 /* Coordinate system:
00046  *  Origin is bottom of main chassis, center horizontally
00047  *    Z OO==> Y
00048  *      ||
00049  *      \/ X
00050  */
00051 
00052 enum Groups {GROUP_ALL, GROUP_TIBIA, GROUP_FEMUR, GROUP_COXA, 
00053             GROUP_RIGHT, GROUP_LEFT, 
00054             GROUP_FRONT, GROUP_MIDDLE, GROUP_BACK};
00055 
00056 class hexapod
00057 {
00058 public:
00059     // both sets of angles are relative to leg root and leg angle
00060     float servoangle[18]; // in degrees, 0 to 300 or so (subject to more constraint)
00061     float angle[18]; // in radians, direction and offset corrected
00062     bool changed[18];
00063     // hexapod body
00064     float length[3]; // length of coxa, femur, tibia
00065     float femurangle, tibiaangle;
00066     float coxaangle; // rajout personnel en fonction de la configuration de lexy
00067     float angleub[3], anglelb[3]; // angle bounds
00068     float legpos[6][3]; // root of leg in xyz
00069     float legpos1[6][3]; // default resting position of leg
00070     float legang[6]; // root angle of leg
00071 
00072     // for walking
00073     float time, speed;
00074     float smoothspeed, fdf;
00075     float turning, smoothturning;
00076     float standheight;
00077     float sweepmodifier, speedmodifier, maxsweep;
00078 
00079     // for safestand
00080     float sstime;
00081     bool ssrunning;
00082     float ssx0[6], ssy0[6], ssz0[6]; // initial positions
00083 
00084     // dead-reckoning
00085     float dr_xpos, dr_ypos, dr_ang;
00086 
00087     bool debug;
00088 
00089     // for walking
00090     Bezier b_walk_up, b_walk_down;
00091 
00092     hexapod (bool debugflag = false);
00093 
00094     void safeStand ();
00095     void step (float dt);
00096     void setAngles ();
00097     void setServoAngles ();
00098     bool IKSolve (int leg, float *target);
00099     void FKSolve (int leg, float *angles, float *pos);
00100     void stand ();
00101     void sit ();
00102 
00103 };
00104 #endif