Julia DESMAZES / Mbed 2 deprecated Hexapode

Sorfware for Lexy ( Hexapode )

Dependencies:   mbed BLE_API X_NUCLEO_IDB0XA1 MODSERIAL

LIB/Hexapod.h@5:58acbceb4f9e, 2016-08-20 (annotated)

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Essenceia 1:8bab9152933e 1 #ifndef HEXAPOD_H
Essenceia 1:8bab9152933e 2 #define HEXAPOD_H
Essenceia 1:8bab9152933e 3 #include <math.h>
Essenceia 1:8bab9152933e 4 #include "Bezier.h"
Essenceia 1:8bab9152933e 5 #ifndef PI
Essenceia 1:8bab9152933e 6 #define PI 3.14159265
Essenceia 1:8bab9152933e 7 #endif
Essenceia 1:8bab9152933e 8 #define DEGTORAD 0.01745329251
Essenceia 1:8bab9152933e 9 #define RADTODEG 57.2957795131
Essenceia 1:8bab9152933e 10 #define MAX_SPEED 1.5
Essenceia 1:8bab9152933e 11 #define SS_DURATION 2.0
Essenceia 1:8bab9152933e 12 #define TURN_TOL 0.005
Essenceia 1:8bab9152933e 13 #define MAXITER 300
Essenceia 1:8bab9152933e 14 // position tolerance in cm
Essenceia 3:13bd725bd47b 15 #define TOLERANCE 0.1
Essenceia 1:8bab9152933e 16 // angle to move things away from bounds by, in radians
Essenceia 1:8bab9152933e 17 #define ANGEPS 0.01
Essenceia 1:8bab9152933e 18
Essenceia 1:8bab9152933e 19 // in turning value per second
Essenceia 1:8bab9152933e 20 #define TURN_SLEW 0.8
Essenceia 1:8bab9152933e 21 #define SPEED_SLEW 0.4
Essenceia 1:8bab9152933e 22
Essenceia 1:8bab9152933e 23 using namespace std;
Essenceia 1:8bab9152933e 24
Essenceia 1:8bab9152933e 25 /* servo IDs:
Essenceia 1:8bab9152933e 26 * back
Essenceia 1:8bab9152933e 27 * 06 10 08 ---- 07 09 11
Essenceia 1:8bab9152933e 28 * 12 16 14 ---- 13 15 17
Essenceia 1:8bab9152933e 29 * 18 04 02 ---- 01 03 05
Essenceia 1:8bab9152933e 30 * front
Essenceia 1:8bab9152933e 31 *
Essenceia 1:8bab9152933e 32 * convert to index:
Essenceia 1:8bab9152933e 33 * (LEG 5) 17 16 15 ---- 06 07 08 (LEG 2)
Essenceia 1:8bab9152933e 34 * (LEG 4) 14 13 12 ---- 03 04 05 (LEG 1)
Essenceia 1:8bab9152933e 35 * (LEG 3) 11 10 09 ---- 00 01 02 (LEG 0)
Essenceia 1:8bab9152933e 36 *
Essenceia 1:8bab9152933e 37 * tibia, femur, coxa ---- coxa, femur, tibia
Essenceia 1:8bab9152933e 38 *
Essenceia 1:8bab9152933e 39 * limits (ignoring intersection with other limbs):
Essenceia 1:8bab9152933e 40 * coxa: 245 - 75
Essenceia 1:8bab9152933e 41 * femur: 250 - 55
Essenceia 1:8bab9152933e 42 * tibia: 215 - 40
Essenceia 1:8bab9152933e 43 */
Essenceia 1:8bab9152933e 44
Essenceia 1:8bab9152933e 45 /* Coordinate system:
Essenceia 1:8bab9152933e 46 * Origin is bottom of main chassis, center horizontally
Essenceia 1:8bab9152933e 47 * Z OO==> Y
Essenceia 1:8bab9152933e 48 * ||
Essenceia 1:8bab9152933e 49 * \/ X
Essenceia 1:8bab9152933e 50 */
Essenceia 1:8bab9152933e 51
Essenceia 1:8bab9152933e 52 enum Groups {GROUP_ALL, GROUP_TIBIA, GROUP_FEMUR, GROUP_COXA,
Essenceia 1:8bab9152933e 53 GROUP_RIGHT, GROUP_LEFT,
Essenceia 1:8bab9152933e 54 GROUP_FRONT, GROUP_MIDDLE, GROUP_BACK};
Essenceia 1:8bab9152933e 55
Essenceia 1:8bab9152933e 56 class hexapod
Essenceia 1:8bab9152933e 57 {
Essenceia 1:8bab9152933e 58 public:
Essenceia 1:8bab9152933e 59 // both sets of angles are relative to leg root and leg angle
Essenceia 1:8bab9152933e 60 float servoangle[18]; // in degrees, 0 to 300 or so (subject to more constraint)
Essenceia 1:8bab9152933e 61 float angle[18]; // in radians, direction and offset corrected
Essenceia 1:8bab9152933e 62 bool changed[18];
Essenceia 1:8bab9152933e 63 // hexapod body
Essenceia 1:8bab9152933e 64 float length[3]; // length of coxa, femur, tibia
Essenceia 1:8bab9152933e 65 float femurangle, tibiaangle;
Essenceia 1:8bab9152933e 66 float coxaangle; // rajout personnel en fonction de la configuration de lexy
Essenceia 1:8bab9152933e 67 float angleub[3], anglelb[3]; // angle bounds
Essenceia 1:8bab9152933e 68 float legpos[6][3]; // root of leg in xyz
Essenceia 1:8bab9152933e 69 float legpos1[6][3]; // default resting position of leg
Essenceia 1:8bab9152933e 70 float legang[6]; // root angle of leg
Essenceia 1:8bab9152933e 71
Essenceia 1:8bab9152933e 72 // for walking
Essenceia 1:8bab9152933e 73 float time, speed;
Essenceia 1:8bab9152933e 74 float smoothspeed, fdf;
Essenceia 1:8bab9152933e 75 float turning, smoothturning;
Essenceia 1:8bab9152933e 76 float standheight;
Essenceia 1:8bab9152933e 77 float sweepmodifier, speedmodifier, maxsweep;
Essenceia 1:8bab9152933e 78
Essenceia 1:8bab9152933e 79 // for safestand
Essenceia 1:8bab9152933e 80 float sstime;
Essenceia 1:8bab9152933e 81 bool ssrunning;
Essenceia 1:8bab9152933e 82 float ssx0[6], ssy0[6], ssz0[6]; // initial positions
Essenceia 1:8bab9152933e 83
Essenceia 1:8bab9152933e 84 // dead-reckoning
Essenceia 1:8bab9152933e 85 float dr_xpos, dr_ypos, dr_ang;
Essenceia 1:8bab9152933e 86
Essenceia 1:8bab9152933e 87 bool debug;
Essenceia 1:8bab9152933e 88
Essenceia 1:8bab9152933e 89 // for walking
Essenceia 1:8bab9152933e 90 Bezier b_walk_up, b_walk_down;
Essenceia 1:8bab9152933e 91
Essenceia 1:8bab9152933e 92 hexapod (bool debugflag = false);
Essenceia 1:8bab9152933e 93
Essenceia 1:8bab9152933e 94 void safeStand ();
Essenceia 1:8bab9152933e 95 void step (float dt);
Essenceia 1:8bab9152933e 96 void setAngles ();
Essenceia 1:8bab9152933e 97 void setServoAngles ();
Essenceia 1:8bab9152933e 98 bool IKSolve (int leg, float *target);
Essenceia 1:8bab9152933e 99 void FKSolve (int leg, float *angles, float *pos);
Essenceia 1:8bab9152933e 100 void stand ();
Essenceia 1:8bab9152933e 101 void sit ();
Essenceia 1:8bab9152933e 102
Essenceia 1:8bab9152933e 103 };
Essenceia 1:8bab9152933e 104 #endif