Si además quieres enviarnos un Artículo para el Blog y redes sociales, pulsa el siguiente botón:
Bueno soy nuevo en el foro me llamo Daniel y este en mi nuevo proyecto, se trata de un Hexápodo cortado a láser ya tengo las piezas cortadas, y casi todos los accesorios (tornillos, tirantes, tuercas, pasadores de nylon, tapones para las patas, etc.) llevo tiempo queriendo hacer uno pero la verdad eso de ponerte a cortar pvc es un co..., ahora tengo la posibilidad de cortarlo a láser y de aluminio de 3mm la verdad es que el corte por láser es increíble. Bueno hay os lo dejo a ve que os parece.
Para los k esten interesados en el codigo del hexapodo para controlarlo por medio del mando de la ps2, utilizando un pic 4550 decidlo y lo pongo en el foro.
Un saludo
Yo, aunque aun ni lo he montado... 😳 pero estoy interesado, a ver si algún día me animase.
Ok.
Gracias danluc y ave_fenix.
Trastearé un poco a ver que tal.
Gracias Danluc. Ahora funciona perfectamente. Sólo ha sido necesario modificar el sentido de 2 servos (Tengo servos mezclados de varios tipos, pero en breve tendré todos iguales).
Bueno pues hay va el codigo esta hecho en ccs, en breve pondre esquema y placa.
#include <18F4550.h>
#fuses NOPROTECT, NOBROWNOUT, NOWDT, NODEBUG, NOLVP, NOSTVREN, NOWRTC, NOWRTB, NOEBTR
#fuses NOCPB, MCLR, NOEBTRB, NOXINST, HSPLL, PLL3, NOUSBDIV, NOVREGEN, NOICPRT
#include <math.h>
#include <stdarg.h>
#include <stdlib.h>
#use delay(clock=48000000)
#use rs232(baud=115200,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=8)
//#use SPI(MASTER,BAUD=10000,MODE=2,BITS=8)
static long counter;
#include <Tones.c>
#int_TIMER2
TIMER2_isr()
{
counter++;
}
/*#define bitrev(c) c = (c & 0x0F) << 4 | (c & 0xF0) >> 4;
c = (c & 0x33) << 2 | (c & 0xCC) >> 2;
c = (c & 0x55) << 1 | (c & 0xAA) >> 1;
*/
//--------------------------------------------------------------------
//------------------Declaracion de funciones--------------------------
void LeerData(int8 datos ,...);
void EnvComand(int8 comando ,...);
void Ps2Input(void);
void WriteLEDs(void);
void GetSinCos(float AngleDeg);
void GetBoogTan(signed long BoogTanX, signed long BoogTanY);
void BodyIK(signed long PosX,signed long PosZ,signed long PosY,signed int BodyOffsetX,signed int BodyOffsetZ,signed int RotationY);
void LegIK(signed long IKFeetPosX, signed long IKFeetPosY, signed long IKFeetPosZ);
void GaitSelect(void);
void GaitSeq(void);
void CheckAngles(void);
void ServoDriver(void);
void ServoSetup(void);
void Gait(int GaitLegNr, signed int p_GaitPosX, signed int p_GaitPosY, signed int p_GaitPosZ, signed int p_GaitRotY);
void BalCalcOneLeg(signed long PosX, signed long PosZ, signed long PosY, signed int BodyOffsetX, signed int BodyOffsetZ);
void BalanceBody(void);
void Setup(void);
//[PS2 Controller]
#define PS2DAT PIN_B0 //PS2 Controller DAT (Brown)
#define PS2CMD PIN_B3 //PS2 controller CMD (Orange)
#define PS2SEL PIN_B2 //PS2 Controller SEL (Blue)
#define PS2CLK PIN_B1 //PS2 Controller CLK (White)
int8 const PadMode = 0x79;
//--------------------------------------------------------------------
//[PIN NUMBERS]
static int const RFCoxaPin = 16; //Front Right leg Hip Horizontal
static int const RFFemurPin = 17; //Front Right leg Hip Vertical
static int const RFTibiaPin = 18; //Front Right leg Knee
static int const RMCoxaPin = 20; //Middle Right leg Hip Horizontal
static int const RMFemurPin = 21; //Middle Right leg Hip Vertical
static int const RMTibiaPin = 22; //Middle Right leg Knee
static int const RRCoxaPin = 28; //Rear Right leg Hip Horizontal
static int const RRFemurPin = 29; //Rear Right leg Hip Vertical
static int const RRTibiaPin = 30; //Rear Right leg Knee
static int const LFCoxaPin = 0; //Front Left leg Hip Horizontal
static int const LFFemurPin = 1; //Front Left leg Hip Vertical
static int const LFTibiaPin = 2; //Front Left leg Knee
static int const LMCoxaPin = 4; //Middle Left leg Hip Horizontal
static int const LMFemurPin = 5; //Middle Left leg Hip Vertical
static int const LMTibiaPin = 6; //Middle Left leg Knee
static int const LRCoxaPin = 12; //Rear Left leg Hip Horizontal
static int const LRFemurPin = 13; //Rear Left leg Hip Vertical
static int const LRTibiaPin = 14; //Rear Left leg Knee
//--------------------------------------------------------------------
//[MIN/MAX ANGLES]
static signed int const RRCoxa_MIN =-78; //Mechanical limits of the Right Rear Leg
static signed int const RRCoxa_MAX = 60;
static signed int const RRFemur_MIN=-73;
static signed int const RRFemur_MAX= 66;
static signed int const RRTibia_MIN=-73;
static signed int const RRTibia_MAX= 68;
static signed int const RMCoxa_MIN =-56; //Mechanical limits of the Right Middle Leg
static signed int const RMCoxa_MAX = 33;
static signed int const RMFemur_MIN=-66;
static signed int const RMFemur_MAX= 68;
static signed int const RMTibia_MIN=-72;
static signed int const RMTibia_MAX= 69;
static signed int const RFCoxa_MIN =-44; //Mechanical limits of the Right Front Leg
static signed int const RFCoxa_MAX = 64;
static signed int const RFFemur_MIN=-70;
static signed int const RFFemur_MAX= 67;
static signed int const RFTibia_MIN=-68;
static signed int const RFTibia_MAX= 65;
static signed int const LRCoxa_MIN =-64; //Mechanical limits of the Left Rear Leg
static signed int const LRCoxa_MAX = 51;
static signed int const LRFemur_MIN=-73;
static signed int const LRFemur_MAX= 69;
static signed int const LRTibia_MIN=-71;
static signed int const LRTibia_MAX= 72;
static signed int const LMCoxa_MIN =-50; //Mechanical limits of the Left Middle Leg
static signed int const LMCoxa_MAX = 30;
static signed int const LMFemur_MIN=-64;
static signed int const LMFemur_MAX= 64;
static signed int const LMTibia_MIN=-70;
static signed int const LMTibia_MAX= 70;
static signed int const LFCoxa_MIN =-45; //Mechanical limits of the Left Front Leg
static signed int const LFCoxa_MAX = 64;
static signed int const LFFemur_MIN=-60;
static signed int const LFFemur_MAX= 66;
static signed int const LFTibia_MIN=-70;
static signed int const LFTibia_MAX= 72;
//--------------------------------------------------------------------
//[BODY DIMENSIONS]
static signed int const CoxaLength = 25; //Length of the Coxa [mm]
static signed int const FemurLength= 78; //Length of the Femur [mm]
static signed int const TibiaLength= 110;//Lenght of the Tibia [mm]
static signed int const CoxaAngle = 45; //Default Coxa setup angle
static signed int const RFOffsetX =-43; //Distance X from center of the body to the Right Front coxa
static signed int const RFOffsetZ =-82; //Distance Z from center of the body to the Right Front coxa
static signed int const RMOffsetX =-63; //Distance X from center of the body to the Right Middle coxa
static signed int const RMOffsetZ = 0; //Distance Z from center of the body to the Right Middle coxa
static signed int const RROffsetX =-43; //Distance X from center of the body to the Right Rear coxa
static signed int const RROffsetZ = 82; //Distance Z from center of the body to the Right Rear coxa
static signed int const LFOffsetX = 43; //Distance X from center of the body to the Left Front coxa
static signed int const LFOffsetZ =-82; //Distance Z from center of the body to the Left Front coxa
static signed int const LMOffsetX = 63; //Distance X from center of the body to the Left Middle coxa
static signed int const LMOffsetZ = 0; //Distance Z from center of the body to the Left Middle coxa
static signed int const LROffsetX = 43; //Distance X from center of the body to the Left Rear coxa
static signed int const LROffsetZ = 82; //Distance Z from center of the body to the Left Rear coxa
//====================================================================
//[ANGLES]
static signed long RFCoxaAngle; //Actual Angle of the Right Front Leg
static signed long RFFemurAngle;
static signed long RFTibiaAngle;
static signed long RMCoxaAngle; //Actual Angle of the Right Middle Leg
static signed long RMFemurAngle;
static signed long RMTibiaAngle;
static signed long RRCoxaAngle; //Actual Angle of the Right Rear Leg
static signed long RRFemurAngle;
static signed long RRTibiaAngle;
static signed long LFCoxaAngle; //Actual Angle of the Left Front Leg
static signed long LFFemurAngle;
static signed long LFTibiaAngle;
static signed long LMCoxaAngle; //Actual Angle of the Left Middle Leg
static signed long LMFemurAngle;
static signed long LMTibiaAngle;
static signed long LRCoxaAngle; //Actual Angle of the Left Rear Leg
static signed long LRFemurAngle;
static signed long LRTibiaAngle;
//--------------------------------------------------------------------
//[POSITIONS]
static signed long RFPosX; //Actual Position of the Right Front Leg
static signed long RFPosY;
static signed long RFPosZ;
static signed long RMPosX; //Actual Position of the Right Middle Leg
static signed long RMPosY;
static signed long RMPosZ;
static signed long RRPosX; //Actual Position of the Right Rear Leg
static signed long RRPosY;
static signed long RRPosZ;
static signed long LFPosX; //Actual Position of the Left Front Leg
static signed long LFPosY;
static signed long LFPosZ;
static signed long LMPosX; //Actual Position of the Left Middle Leg
static signed long LMPosY;
static signed long LMPosZ;
static signed long LRPosX; //Actual Position of the Left Rear Leg
static signed long LRPosY;
static signed long LRPosZ;
//--------------------------------------------------------------------
//[VARIABLES]
static int Index; //Index used for freeing the servos
static int SSCDone; //Char to check if SSC is done
//Body position
static signed BodyPosX; //Global Input for the position of the body
static signed long BodyPosY;
static signed BodyPosZ;
//Body Inverse Kinematics
static float SinA, CosA, SinB, CosB, SinG, CosG, Aux;
static float BoogTan;
static signed long TotalX; //Total X distance between the center of the body and the feet
static signed long TotalZ; //Total Z distance between the center of the body and the feet
static signed BodyRotX; //Global Input pitch of the body
static signed BodyRotY; //Global Input rotation of the body
static signed BodyRotZ; //Global Input roll of the body
static signed long BodyIKPosX;
static signed long BodyIKPosY;
static signed long BodyIKPosZ;
//Leg Inverse Kinematics
static short IKSolution; //Output true if the solution is possible
static short IKSolutionWarning;//Output true if the solution is NEARLY possible
static short IKSolutionError; //Output true if the solution is NOT possible
static signed long IKFemurAngle;
static signed long IKTibiaAngle;
static signed long IKCoxaAngle;
//--------------------------------------------------------------------
//[Ps2 Controller]
static int LastButton[2];
static int DualShock[7];
static signed int BodyYShift;
//--------------------------------------------------------------------
//[TIMING]
static long SSCTime; //Time for servo updateslong
static long PrevSSCTime; //Previous time for the servo updates
static long InputTimeDelay;//Delay that depends on the input to get the "sneaking" effect
//--------------------------------------------------------------------
//[GLOABAL]
static short HexOn; //Switch to turn on Phoenix
//--------------------------------------------------------------------
//[gait]
static int GaitType; //Gait type
static int NomGaitSpeed; //Nominal speed of the gait
static signed int GaitPosX;
static signed int GaitPosY;
static signed int GaitPosZ;
static signed int GaitRotY;
static int LegLiftHeight; //Current Travel height
static signed long TravelLengthX; //Current Travel length X
static signed long TravelLengthZ; //Current Travel length Z
static signed long TravelRotationY;//Current Travel Rotation Y
static int TLDivFactor; //Number of steps that a leg is on the floor while walking
static int NrLiftedPos; //Number of positions that a single leg is lifted (1-3)
static short HalfLiftHeigth; //If TRUE the outer positions of the ligted legs will be half height
static int StepsInGait; //Number of steps in gait
static short LastLeg; //TRUE when the current leg is the last leg of the sequence
static int GaitStep; //Actual Gait step
static int RFGaitLegNr; //Init position of the leg
static int RMGaitLegNr; //Init position of the leg
static int RRGaitLegNr; //Init position of the leg
static int LFGaitLegNr; //Init position of the leg
static int LMGaitLegNr; //Init position of the leg
static int LRGaitLegNr; //Init position of the leg
static signed TravelMulti; //Multiplier for the length of the step
static signed RFGaitPosX; //Relative position corresponding to the Gait
static signed RFGaitPosY;
static signed RFGaitPosZ;
static signed RFGaitRotY; //Relative rotation corresponding to the Gait
static signed RMGaitPosX;
static signed RMGaitPosY;
static signed RMGaitPosZ;
static signed RMGaitRotY;
static signed RRGaitPosX;
static signed RRGaitPosY;
static signed RRGaitPosZ;
static signed RRGaitRotY;
static signed LFGaitPosX;
static signed LFGaitPosY;
static signed LFGaitPosZ;
static signed LFGaitRotY;
static signed LMGaitPosX;
static signed LMGaitPosY;
static signed LMGaitPosZ;
static signed LMGaitRotY;
static signed LRGaitPosX;
static signed LRGaitPosY;
static signed LRGaitPosZ;
static signed LRGaitRotY;
//Balance
static short BalanceMode;
static signed long TravelHeightY;
static signed long TotalTransX;
static signed long TotalTransZ;
static signed long TotalTransY;
static signed long TotalYbal;
static signed long TotalXBal;
static signed long TotalZBal;
static signed long TotalY; //Total Y distance between the center of the body and the feet
//Setup
static short SetupMode;
static int RegNo;
static signed int ValReg[32];
//====================================================================
//[MAIN]
void main(){
signed long Tiempo;
port_b_pullups(TRUE);
setup_adc_ports(NO_ANALOGS|VSS_VDD);
setup_wdt(WDT_OFF);
//setup_timer_0(RTCC_INTERNAL);
setup_timer_1(T1_DISABLED);
setup_timer_2(T2_DIV_BY_16,249,3);
enable_interrupts(INT_TIMER2);
enable_interrupts(GLOBAL);
set_tris_d(0x00);
set_tris_b(0xFF);
//[INIT]
//Feet Positions
RFPosX = 53; //Start positions of the Right Front leg
RFPosY = 25;
RFPosZ = -91;
RMPosX = 105; //Start positions of the Right Middle leg
RMPosY = 25;
RMPosZ = 0;
RRPosX = 53; //Start positions of the Right Rear leg
RRPosY = 25;
RRPosZ = 91;
LFPosX = 53; //Start positions of the Left Front leg
LFPosY = 25;
LFPosZ = -91;
LMPosX = 105; //Start positions of the Left Middle leg
LMPosY = 25;
LMPosZ = 0;
LRPosX = 53; //Start positions of the Left Rear leg
LRPosY = 25;
LRPosZ = 91;
//Body Positions
BodyPosX = 0;
BodyPosY = 0;
BodyPosZ = 0;
//Body Rotations
BodyRotX = 0;
BodyRotY = 0;
BodyRotZ = 0;
//Gait
GaitType = 0;
LegLiftHeight = 50;
GaitStep = 1;
GaitSelect();
//PS2 controller
output_high(PS2CLK);
LastButton[0] = 255;
LastButton[1] = 255;
BodyYShift = 0;
//SSC
SSCTime = 150;
HexOn = False;
while (1){
//Setup
if (SetupMode){
Setup();
}
//Start time
counter = 0;
//Gait
GaitSeq();
//Balance calculations
TotalTransX = 0; //reset values used for calculation of balance
TotalTransZ = 0;
TotalTransY = 0;
TotalXBal = 0;
TotalYBal = 0;
TotalZBal = 0;
if (BalanceMode>0){
BalCalcOneLeg(-RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BodyPosZ+RFGaitPosZ,RFGaitPosY, RFOffsetX, RFOffsetZ);
BalCalcOneLeg(-RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BodyPosZ+RMGaitPosZ,RMGaitPosY, RMOffsetX, RMOffsetZ);
BalCalcOneLeg(-RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BodyPosZ+RRGaitPosZ,RRGaitPosY, RROffsetX, RROffsetZ);
BalCalcOneLeg(LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BodyPosZ+LFGaitPosZ,LFGaitPosY, LFOffsetX, LFOffsetZ);
BalCalcOneLeg(LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BodyPosZ+LMGaitPosZ,LMGaitPosY, LMOffsetX, LMOffsetZ);
BalCalcOneLeg(LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BodyPosZ+LRGaitPosZ,LRGaitPosY, LROffsetX, LROffsetZ);
BalanceBody();
}
//Reset IKsolution indicators
IKSolution = False;
IKSolutionWarning = False;
IKSolutionError = False;
//Right Front leg
BodyIK(-RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BodyPosZ+RFGaitPosZ,RFPosY+BodyPosY+BodyYShift+RFGaitPosY, RFOffsetX, RFOffsetZ, RFGaitRotY);
LegIK(RFPosX-BodyPosX+BodyIKPosX-RFGaitPosX, RFPosY+BodyPosY+BodyYShift-BodyIKPosY+RFGaitPosY, RFPosZ+BodyPosZ-BodyIKPosZ+RFGaitPosZ);
RFCoxaAngle = IKCoxaAngle + CoxaAngle; //Angle for the basic setup for the front leg
RFFemurAngle = IKFemurAngle;
RFTibiaAngle = IKTibiaAngle;
//Right Middle leg
BodyIK(-RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BodyPosZ+RMGaitPosZ,RMPosY+BodyPosY+BodyYShift+RMGaitPosY, RMOffsetX, RMOffsetZ, RMGaitRotY);
LegIK(RMPosX-BodyPosX+BodyIKPosX-RMGaitPosX, RMPosY+BodyPosY+BodyYShift-BodyIKPosY+RMGaitPosY, RMPosZ+BodyPosZ-BodyIKPosZ+RMGaitPosZ);
RMCoxaAngle = IKCoxaAngle;
RMFemurAngle = IKFemurAngle;
RMTibiaAngle = IKTibiaAngle;
//Right Rear leg
BodyIK(-RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BodyPosZ+RRGaitPosZ,RRPosY+BodyPosY+BodyYShift+RRGaitPosY, RROffsetX, RROffsetZ, RRGaitRotY);
LegIK(RRPosX-BodyPosX+BodyIKPosX-RRGaitPosX, RRPosY+BodyPosY+BodyYShift-BodyIKPosY+RRGaitPosY, RRPosZ+BodyPosZ-BodyIKPosZ+RRGaitPosZ);
RRCoxaAngle = IKCoxaAngle - CoxaAngle; //Angle for the basic setup for the Rear leg
RRFemurAngle = IKFemurAngle;
RRTibiaAngle = IKTibiaAngle;
//Left Front leg
BodyIK(LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BodyPosZ+LFGaitPosZ,LFPosY+BodyPosY+BodyYShift+LFGaitPosY, LFOffsetX, LFOffsetZ, LFGaitRotY);
LegIK(LFPosX+BodyPosX-BodyIKPosX+LFGaitPosX, LFPosY+BodyPosY+BodyYShift-BodyIKPosY+LFGaitPosY, LFPosZ+BodyPosZ-BodyIKPosZ+LFGaitPosZ);
LFCoxaAngle = IKCoxaAngle + CoxaAngle; //Angle for the basic setup for the front leg
LFFemurAngle = IKFemurAngle;
LFTibiaAngle = IKTibiaAngle;
//Left Middle leg
BodyIK(LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BodyPosZ+LMGaitPosZ,LMPosY+BodyPosY+BodyYShift+LMGaitPosY, LMOffsetX, LMOffsetZ, LMGaitRotY);
LegIK(LMPosX+BodyPosX-BodyIKPosX+LMGaitPosX, LMPosY+BodyPosY+BodyYShift-BodyIKPosY+LMGaitPosY, LMPosZ+BodyPosZ-BodyIKPosZ+LMGaitPosZ);
LMCoxaAngle = IKCoxaAngle;
LMFemurAngle = IKFemurAngle;
LMTibiaAngle = IKTibiaAngle;
//Left Rear leg
BodyIK(LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BodyPosZ+LRGaitPosZ,LRPosY+BodyPosY+BodyYShift+LRGaitPosY, LROffsetX, LROffsetZ, LRGaitRotY);
LegIK(LRPosX+BodyPosX-BodyIKPosX+LRGaitPosX, LRPosY+BodyPosY+BodyYShift-BodyIKPosY+LRGaitPosY, LRPosZ+BodyPosZ-BodyIKPosZ+LRGaitPosZ);
LRCoxaAngle = IKCoxaAngle - CoxaAngle; //Angle for the basic setup for the front leg
LRFemurAngle = IKFemurAngle;
LRTibiaAngle = IKTibiaAngle;
CheckAngles();
//Read input
Ps2Input();
//GOSUB ReadButtons ;I/O used by the PS2 remote
WriteLeds(); //I/O used by the PS2 remote
if (HexOn) {
//Wait for previous commands to be completed while walking
if ((ABS(TravelLengthX)>2 || ABS(TravelLengthZ)>2 || ABS(TravelRotationY*2)>2)){
Tiempo = PrevSSCTime - counter - 16;
//printf("Tiempo=%Ld PrevSSCTime=%Lu Counter=%Ld %c", Tiempo,PrevSSCTime,contador,13);
if (Tiempo>0){
delay_ms(Tiempo);
}
else
delay_ms(1); //Min 1 ensures that there alway is a value in the pause command
SSCTime = NomGaitSpeed + (InputTimeDelay*2);
}
else{
SSCTime = 200; //NomGaitSpeed
}
ServoDriver();
}
else{
//Turn the bot off
SSCTime = 1000;
ServoDriver();
//FreeServos();
}
}
}
//--------------------------------------------------------------------
//[WriteLEDs] Updates the state of the leds
void WriteLEDs(void){
if (IKSolutionWarning)
output_low(PIN_D3);
else
output_high(PIN_D3);
if (IKSolutionError)
output_low(PIN_D2);
else
output_high(PIN_D2);
if (HexOn)
output_low(PIN_D0);
else
output_high(PIN_D0);
if (GaitType == 3)
output_low(PIN_D1);
else
output_high(PIN_D1);
}
//--------------------------------------------------------------------
//[PS2Input] Lee el mando de la ps2
void EnvComand(int comando,...){
int x, i;
va_list listcomando; // Declaramos variables de lista de argumentos
va_start(listcomando, comando);// Inicializamos lista de comandos
while (comando--){
x = va_arg(listcomando, int);//Cargamos el comado que toque en x
delay_us(20);
for(i=0;i<=7;i++){
output_low(PS2CLK);//Bajamos el reloj
delay_us(1);
if(bit_test(x, i)==0)
output_low(PS2CMD);
else
output_high(PS2CMD);
delay_us(1);
output_high(PS2CLK);
delay_us(1);
}
}
}
void LeerData(int datos,...){
int x, i;
signed int c;
va_list listdatos; // Declaramos variables de lista de argumentos
va_start(listdatos, datos);// Inicializamos lista de argumentos
c=-1;
while (datos--){
c++;
x = va_arg(listdatos, int);//Cargamos el comado que toque en x
delay_us(20);
for(i=0;i<=7;i++){
output_low(PS2CLK);//Bajamos el reloj
delay_us(1);
if(input(PS2DAT)==0)
bit_clear(x, i);
else
bit_set(x, i);
delay_us(1);
output_high(PS2CLK);
delay_us(1);
}
DualShock[c] = x;
}
}
void Ps2Input(void){
int re, aux, i;
output_high(PS2CLK);
output_low(PS2SEL);
delay_us(14);
EnvComand(1,0x01);
LeerData(1,0x00);
output_high(PS2SEL);
re = DualShock[0];
output_low(PS2SEL);
delay_us(14);
EnvComand(1,0x01);
EnvComand(1,0x42);
LeerData(7,0x00);
output_high(PS2SEL);
if (re != 0x79){
output_low(PS2SEL);
EnvComand(5,0x01,0x43,0x00,0x01,0x00);
output_high(PS2SEL);
Delay_ms(10);
output_low(PS2SEL);
EnvComand(9,0x01,0x44,0x00,0x01,0x03,0x00,0x00,0x00,0x00);
output_high(PS2SEL);
Delay_ms(10);
output_low(PS2SEL);
EnvComand(9,0x01,0x4F,0x00,0xFF,0xFF,0x03,0x00,0x00,0x00);
output_high(PS2SEL);
Delay_ms(10);
output_low(PS2SEL);
EnvComand(9,0x01,0x43,0x00,0x00,0x5A,0x5A,0x5A,0x5A,0x5A);
output_high(PS2SEL);
Delay_ms(10);
output_low(PS2SEL);
EnvComand(9,0x01,0x43,0x00,0x00,0x00,0x00,0x00,0x00,0x00);
Delay_ms(100);
output_high(PS2SEL);
generate_tone(4000,20);
generate_tone(4500,20);
generate_tone(7000,20);
return;
}
if(bit_test(DualShock[1],3) == 0 & bit_test(LastButton[0],3)){ //Start Button test
if(HexOn){
//Turn off
generate_tone(5000,100);
generate_tone(4500,80);
generate_tone(4000,60);
BodyPosX = 0;
BodyPosY = 0;
BodyPosZ = 0;
BodyRotX = 0;
BodyRotY = 0;
BodyRotZ = 0;
TravelLengthX = 0;
TravelLengthZ = 0;
TravelRotationY = 0;
SSCTime = 1000;
ServoDriver();
HexOn = False;
}
else{
//Turn on
generate_tone(4000,60);
generate_tone(4500,80);
generate_tone(5000,100);
SSCTime = 200;
HexOn = True;
}
}
if (HexOn){
if (bit_test(DualShock[1],0) == 0 & bit_test(LastButton[0],0)){ //Select Button test
if (SetupMode){
ServoSetup();
goto salir;
}
if (TravelLengthX==0 & TravelLengthZ==0 & TravelRotationY==0){
//Switch to next Gait type
if (GaitType<4){
generate_tone(4000,50);
GaitType = GaitType+1;
}
else{
generate_tone(4000,50);
generate_tone(4500,50);
GaitType = 0;
}
GaitSelect();
}
}
if (bit_test(DualShock[1],4) == 0){ //Up Button test
if (SetupMode){
if (ValReg[RegNo]<100)
ValReg[RegNo]++;
}
else
BodyPosY++;
}
if (bit_test(DualShock[1],6) == 0){ //Down Button test
if (SetupMode){
if (ValReg[RegNo]>-100)
ValReg[RegNo]--;
}
else
BodyPosY--;
}
if (bit_test(DualShock[1],5) == 0 & bit_test(LastButton[0],5)){ //Right Button test
if (RegNo<31)
RegNo++;
}
if (bit_test(DualShock[1],7) == 0 & bit_test(LastButton[0],7)){ //Left Button test
if (RegNo>0)
RegNo--;
}
if (bit_test(DualShock[2],4) == 0 & bit_test(LastButton[1],4)){ //Triangle Button test
BodyPosY = 35;
}
if (bit_test(DualShock[2],5) == 0 & bit_test(LastButton[1],5)){ //Circle Button test
BodyPosY = 0;
}
if (bit_test(DualShock[2],7) == 0 & bit_test(LastButton[1],7)){ //Square Button test
if (BalanceMode == 0){
BalanceMode = 1;
generate_tone(4000,100);
generate_tone(8000,100);
}
else{
BalanceMode = 0;
generate_tone(3000,250);
}
}
if (bit_test(DualShock[2],6) == 0 & bit_test(LastButton[1],6)){ //X Button test (se utiliza para ajuste fino de las articulaciones)
if (SetupMode == 0){
SetupMode = 1;
generate_tone(2000,100);
generate_tone(10000,100);
}
else{
SetupMode = 0;
generate_tone(8000,250);
}
}
if (bit_test(DualShock[2],3) == 0) //R1 Button test
LegLiftHeight = 80;
else
LegLiftHeight = 50;
BodyYShift = 0;
if (bit_test(DualShock[2],2) == 0){ //L1 Button test
BodyPosX = ((signed)Dualshock[5] - 128)/2;
BodyPosZ = (((signed)Dualshock[6] - 128)/3)*(-1);
BodyYShift = (((signed)Dualshock[4] - 128)/2)*(-1);
if (BodyYShift<(BodyPosY-10))
BodyYShift = BodyPosY-10;
}
else{
if (bit_test(DualShock[2],0) == 0){ //L2 Button test
BodyRotX = ((signed)Dualshock[6] - 128)/3;
BodyRotY = ((signed)Dualshock[3] - 128)/8;
BodyRotZ = ((signed)Dualshock[5] - 128)/10;
BodyYShift = (((signed)Dualshock[4] - 128)/2)*(-1);
if (BodyYShift<(BodyPosY-10))
BodyYShift = BodyPosY-10;
}
else{ //Walk
if (bit_test(DualShock[2],1) == 0){ //R2 Button test
TravelLengthX = -((signed long)Dualshock[5] - 128);
TravelLengthZ = ((signed long)Dualshock[6] - 128);
}
else{
TravelLengthX = -((signed long)Dualshock[5] - 128)/2;
TravelLengthZ = ((signed long)Dualshock[6] - 128)/2;
}
TravelRotationY = -((signed long)Dualshock[3] - 128)/4;
}
//Calculate walking time delay
InputTimeDelay = 128 - ABS(Dualshock[5] - 128);
//printf("InputTimeDelay0=%Lu %c",InputTimeDelay,13);
if (128 - ABS(Dualshock[6] - 128)<InputTimeDelay){
InputTimeDelay = 128 - ABS(Dualshock[6] - 128);
//printf("InputTimeDelay1=%Lu %c",InputTimeDelay,13);
}
else{
if (128 - ABS(Dualshock[3] - 128)<InputTimeDelay){
InputTimeDelay = 128 - ABS(Dualshock[3] - 128);
//printf("InputTimeDelay2=%Lu %c",InputTimeDelay,13);
}
}
}
salir:
LastButton[0] = DualShock[1];
LastButton[1] = DualShock[2];
}
}
//--------------------------------------------------------------------
//[GAIT Select]
void GaitSelect(void){
//Gait selector
if (GaitType == 0){ //Ripple Gait 6 steps
LRGaitLegNr = 1;
RFGaitLegNr = 2;
LMGaitLegNr = 3;
RRGaitLegNr = 4;
LFGaitLegNr = 5;
RMGaitLegNr = 6;
NrLiftedPos = 1;
TLDivFactor = 4;
StepsInGait = 6;
NomGaitSpeed = 150;
}
if (GaitType == 1){ //Ripple Gait 12 steps
LRGaitLegNr = 1;
RFGaitLegNr = 3;
LMGaitLegNr = 5;
RRGaitLegNr = 7;
LFGaitLegNr = 9;
RMGaitLegNr = 11;
NrLiftedPos = 3;
HalfLiftHeigth = TRUE;
TLDivFactor = 8;
StepsInGait = 12;
NomGaitSpeed = 100;
}
if (GaitType == 2){ //Quadripple 9 steps
LRGaitLegNr = 1;
RFGaitLegNr = 2;
LMGaitLegNr = 4;
RRGaitLegNr = 5;
LFGaitLegNr = 7;
RMGaitLegNr = 8;
NrLiftedPos = 2;
HalfLiftHeigth = FALSE;
TLDivFactor = 6;
StepsInGait = 9;
NomGaitSpeed = 150;
}
if (GaitType == 3){ //Tripod 4 steps
LRGaitLegNr = 3;
RFGaitLegNr = 1;
LMGaitLegNr = 1;
RRGaitLegNr = 1;
LFGaitLegNr = 3;
RMGaitLegNr = 3;
NrLiftedPos = 1;
TLDivFactor = 2;
StepsInGait = 4;
NomGaitSpeed = 150;
}
if (GaitType == 4){ //Tripod 6 steps
LRGaitLegNr = 4;
RFGaitLegNr = 1;
LMGaitLegNr = 1;
RRGaitLegNr = 1;
LFGaitLegNr = 4;
RMGaitLegNr = 4;
NrLiftedPos = 2;
HalfLiftHeigth = FALSE;
TLDivFactor = 4;
StepsInGait = 6;
NomGaitSpeed = 150;
}
}
//--------------------------------------------------------------------
//[GAIT Sequence]
void GaitSeq(void){
//Calculate Gait sequence
LastLeg = FALSE;
Gait(LRGaitLegNr, LRGaitPosX, LRGaitPosY, LRGaitPosZ, LRGaitRotY);
LRGaitPosX = GaitPosX;
LRGaitPosY = GaitPosY;
LRGaitPosZ = GaitPosZ;
LRGaitRotY = GaitRotY;
Gait(RFGaitLegNr, RFGaitPosX, RFGaitPosY, RFGaitPosZ, RFGaitRotY);
RFGaitPosX = GaitPosX;
RFGaitPosY = GaitPosY;
RFGaitPosZ = GaitPosZ;
RFGaitRotY = GaitRotY;
Gait(LMGaitLegNr, LMGaitPosX, LMGaitPosY, LMGaitPosZ, LMGaitRotY);
LMGaitPosX = GaitPosX;
LMGaitPosY = GaitPosY;
LMGaitPosZ = GaitPosZ;
LMGaitRotY = GaitRotY;
Gait(RRGaitLegNr, RRGaitPosX, RRGaitPosY, RRGaitPosZ, RRGaitRotY);
RRGaitPosX = GaitPosX;
RRGaitPosY = GaitPosY;
RRGaitPosZ = GaitPosZ;
RRGaitRotY = GaitRotY;
Gait(LFGaitLegNr, LFGaitPosX, LFGaitPosY, LFGaitPosZ, LFGaitRotY);
LFGaitPosX = GaitPosX;
LFGaitPosY = GaitPosY;
LFGaitPosZ = GaitPosZ;
LFGaitRotY = GaitRotY;
LastLeg = TRUE;
Gait(RMGaitLegNr, RMGaitPosX, RMGaitPosY, RMGaitPosZ, RMGaitRotY);
RMGaitPosX = GaitPosX;
RMGaitPosY = GaitPosY;
RMGaitPosZ = GaitPosZ;
RMGaitRotY = GaitRotY;
}
//--------------------------------------------------------------------
//[GAIT]
void Gait(int GaitLegNr, signed int p_GaitPosX, signed int p_GaitPosY, signed int p_GaitPosZ, signed int p_GaitRotY){
short GaitInMotion;
GaitInMotion = ((abs(TravelLengthX)>2) || (abs(TravelLengthZ)>2) || (abs(TravelRotationY)>2) );
/*printf("GaitLegNr=%u %c",GaitLegNr,13);
printf("NrLiftedPos=%u %c",NrLiftedPos,13);
printf("GaitStep=%u %c",GaitStep,13);
printf("GaitInMotion=%u %c",GaitInMotion,13);
printf("p_GaitPosX=%d %c",P_GaitPosX,13);
printf("p_GaitPosY=%d %c",P_GaitPosY,13);
printf("p_GaitPosZ=%d %c",P_GaitPosZ,13);
printf("p_GaitRotY=%d %c",P_GaitRotY,13);*/
//Check IF the Gait is in motion
//Leg middle up position
//Gait in motion Gait NOT in motion, return to home position
if ((GaitInMotion & (NrLiftedPos==1 || NrLiftedPos==3) & GaitStep==GaitLegNr) || (GaitInMotion==FALSE & GaitStep==GaitLegNr & ((ABS(p_GaitPosX)>2) || (ABS(p_GaitPosZ)>2) || (ABS(p_GaitRotY)>2)))){ //Up
GaitPosX = 0;
GaitPosY = -LegLiftHeight;
GaitPosZ = 0;
GaitRotY = 0;
//printf("Leg middle up position%c",13);
}
else{
//Optional Half heigth Rear
if (((NrLiftedPos==2 & GaitStep==GaitLegNr) || (NrLiftedPos==3 & (GaitStep==GaitLegNr-1 || GaitStep==GaitLegNr+(StepsInGait-1)))) & GaitInMotion){
GaitPosX = -(TravelLengthX/2);
GaitPosY = -(LegLiftHeight/(HalfLiftHeigth+1));
GaitPosZ = -(TravelLengthZ/2);
GaitRotY = -(TravelRotationY/2);
//printf("GaitRotY=%d %c",GaitRotY,13);
//printf("Optional Half heigth%c",13);
}
else{
//Optional half heigth front
if ((NrLiftedPos>=2) & (GaitStep==(GaitLegNr+1) || (GaitStep==(GaitLegNr-(StepsInGait-1)))) & GaitInMotion){
GaitPosX = (TravelLengthX/2);
GaitPosY = -(LegLiftHeight/(HalfLiftHeigth+1));
GaitPosZ = (TravelLengthZ/2);
GaitRotY = (TravelRotationY/2);
//printf("LegLiftHeight=%u HalfLifHeigth=%u--------------------GaitPosY=%d %c",LegLiftHeight,HalfLiftHeigth,GaitPosY,13);
//printf("Optional half heigth front%c",13);
}
else{
//Leg front down position
if ((GaitStep==(GaitLegNr+NrLiftedPos) || GaitStep==(GaitLegNr-(StepsInGait-NrLiftedPos))) & p_GaitPosY<0){
GaitPosX = (TravelLengthX/2);
GaitPosY = 0;
GaitPosZ = (TravelLengthZ/2);
GaitRotY = (TravelRotationY/2);
//printf("GaitRotY=%d %c",GaitRotY,13);
//printf("Leg front down position%c",13);
} //Move body forward
else{
GaitPosX = p_GaitPosX - (TravelLengthX/TLDivFactor);
GaitPosY = 0;
GaitPosZ = p_GaitPosZ - (TravelLengthZ/TLDivFactor);
GaitRotY = p_GaitRotY - (TravelRotationY/TLDivFactor);
//printf("GaitRotY=%d %c",GaitRotY,13);
//printf("Move body to forward%c",13);
}
}
}
}
//Advance to the next step
if (LastLeg){ //The last leg in this step
GaitStep = GaitStep+1;
if (GaitStep>StepsInGait)
GaitStep = 1;
}
}
//--------------------------------------------------------------------
//[BalCalcOneLeg]
void BalCalcOneLeg(signed long PosX, signed long PosZ, signed long PosY, signed int BodyOffsetX, signed int BodyOffsetZ){
//Calculating totals from center of the body to the feet
signed long TotalZ, TotalX, TotalY;
TotalZ = BodyOffsetZ+PosZ;
TotalX = BodyOffsetX+PosX;
TotalY = 150 + PosY; // using the value 150 to lower the centerpoint of rotation 'BodyPosY +
TotalTransY = TotalTransY + PosY;
TotalTransZ = TotalTransZ + TotalZ;
TotalTransX = TotalTransX + TotalX;
GetBoogTan(TotalX, TotalZ);
TotalYbal = TotalYbal + (signed long)Floor((BoogTan*180.0) / 3.141592);
GetBoogTan(TotalX, TotalY);
TotalZbal = TotalZbal + (signed long)Floor((BoogTan*180.0) / 3.141592);
GetBoogTan(TotalZ, TotalY);
TotalXbal = TotalXbal + (signed long)Floor((BoogTan*180.0) / 3.141592);
}
//--------------------------------------------------------------------
//[BalanceBody]
void BalanceBody(void){
TotalTransZ = TotalTransZ/6;
TotalTransX = TotalTransX/6;
TotalTransY = TotalTransY/6;
if (TotalYbal < -180) //Tangens fix caused by +/- 180 deg
TotalYbal = TotalYbal + 360;
if (TotalZbal < -180) //Tangens fix caused by +/- 180 deg
TotalZbal = TotalZbal + 360;
if (TotalXbal < -180) //Tangens fix caused by +/- 180 deg
TotalXbal = TotalXbal + 360;
//Balance rotation
TotalYBal = TotalYbal/6;
TotalXBal = TotalXbal/6;
TotalZBal = -TotalZbal/6;
//Balance translation
LFGaitPosZ = LFGaitPosZ - TotalTransZ;
LMGaitPosZ = LMGaitPosZ - TotalTransZ;
LRGaitPosZ = LRGaitPosZ - TotalTransZ;
RFGaitPosZ = RFGaitPosZ - TotalTransZ;
RMGaitPosZ = RMGaitPosZ - TotalTransZ;
RRGaitPosZ = RRGaitPosZ - TotalTransZ;
LFGaitPosX = LFGaitPosX - TotalTransX;
LMGaitPosX = LMGaitPosX - TotalTransX;
LRGaitPosX = LRGaitPosX - TotalTransX;
RFGaitPosX = RFGaitPosX - TotalTransX;
RMGaitPosX = RMGaitPosX - TotalTransX;
RRGaitPosX = RRGaitPosX - TotalTransX;
LFGaitPosY = LFGaitPosY - TotalTransY;
LMGaitPosY = LMGaitPosY - TotalTransY;
LRGaitPosY = LRGaitPosY - TotalTransY;
RFGaitPosY = RFGaitPosY - TotalTransY;
RMGaitPosY = RMGaitPosY - TotalTransY;
RRGaitPosY = RRGaitPosY - TotalTransY;
}
//--------------------------------------------------------------------
//[GETSINCOS] Get the sinus and cosinus from the angle +/- multiple circles
//AngleDeg - Input Angle in degrees
//SinA - Output Sinus of AngleDeg
//CosA - Output Cosinus of AngleDeg
//GetSinCos
void GetSinCos(float AngleDeg){
float ABSAngleDeg;
float AngleRad;
//Get the absolute value of AngleDeg
if (AngleDeg < 0.0){
ABSAngleDeg = AngleDeg *-1.0;
//printf("ABSAngleDeg<0=%f %c",ABSAngleDeg,13);
}
else{
ABSAngleDeg = AngleDeg;
//printf("ABSAngleDeg>0=%f %c",ABSAngleDeg,13);
}
//Shift rotation to a full circle of 360 deg -> AngleDeg // 360
if (AngleDeg < 0.0){ //Negative values
AngleDeg = 360.0-(ABSAngleDeg-(360*(floor(ABSAngleDeg/360.0))));
//printf("AngleDeg<0=%f %c",AngleDeg,13);
}
else{ //Positive values
//AngleDeg = floor(ABSAngleDeg/360.0);
//AngleDeg = 360*AngleDeg;
AngleDeg = ABSAngleDeg-(360*(floor(ABSAngleDeg/360.0)));
//printf("AngleDeg>0=%f %c",AngleDeg,13);
}
if (AngleDeg < 180.0){ //Angle between 0 and 180
//Subtract 90 to shift range
//printf("AngleDeg<180=%f %c",AngleDeg,13);
AngleDeg-=90.0;
//printf("AngleDeg<180-90=%f %c",AngleDeg,13);
//Convert degree to radials
AngleRad = (AngleDeg*3.141592)/180.0;
//printf("AngleRad<180=%f Cos(AngleRad)=%f %c",AngleRad,cos(AngleRad),13);
SinA = cos(AngleRad); //Sin o to 180 deg = cos(Angle Rad - 90deg)
CosA = -sin(AngleRad); //Cos 0 to 180 deg = -sin(Angle Rad - 90deg)
}
else{ //Angle between 180 and 360
//Subtract 270 to shift range
//printf("AngleDeg>180=%f %c",AngleDeg,13);
AngleDeg = AngleDeg -270.0;
//printf("AngleDeg>180-170=%f %c",AngleDeg,13);
//Convert degree to radials
AngleRad = (AngleDeg*3.141592)/180.0;
//printf("AngleRad>180=%f Cos(AngleRad)=%f %c",AngleRad,cos(AngleRad),13);
SinA = -cos(AngleRad); //Sin 180 to 360 deg = -cos(Angle Rad - 270deg)
CosA = sin(AngleRad); //Cos 180 to 360 deg = sin(Angle Rad - 270deg)
}
}
//--------------------------------------------------------------------
//[BOOGTAN2] Gets the Inverse Tangus from X/Y with the where Y can be zero or negative
//BoogTanX - Input X
//BoogTanY - Input Y
//BoogTan - Output BOOGTAN2(X/Y)
void GetBoogTan(signed long BoogTanX,signed long BoogTanY){
if (BoogTanX == 0){ // X=0 -> 0 or PI
if (BoogTanY >= 0){
BoogTan = 0.0;
}
else{
BoogTan = 3.141592;
}
}
else{
if (BoogTanY == 0){ //Y=0 -> +/- Pi/2
if (BoogTanX > 0){
BoogTan = 3.141592 / 2.0;
}
else{
BoogTan = -3.141592 / 2.0;
}
}
else{
if (BoogTanY > 0){ //BOOGTAN(X/Y)
BoogTan = atan((float)BoogTanX / (float)BoogTanY);
}
else{
if (BoogTanX > 0){ //BOOGTAN(X/Y) + PI
BoogTan = atan((float)BoogTanX / (float)BoogTanY) + 3.141592;
}
else{ //BOOGTAN(X/Y) - PI
BoogTan = atan((float)BoogTanX / (float)BoogTanY) - 3.141592;
}
}
}
}
}
//--------------------------------------------------------------------
//[BODY INVERSE KINEMATICS]
//BodyRotX - Global Input pitch of the body
//BodyRotY - Global Input rotation of the body
//BodyRotZ - Global Input roll of the body
//RotationY - Input Rotation for the gait
//PosX - Input position of the feet X
//PosZ - Input position of the feet Z
//BodyOffsetX - Input Offset betweeen the body and Coxa X
//BodyOffsetZ - Input Offset betweeen the body and Coxa Z
//SinB - Sin buffer for BodyRotX
//CosB - Cos buffer for BodyRotX
//SinG - Sin buffer for BodyRotZ
//CosG - Cos buffer for BodyRotZ
//BodyIKPosX - Output Position X of feet with Rotation
//BodyIKPosY - Output Position Y of feet with Rotation
//BodyIKPosZ - Output Position Z of feet with Rotation
void BodyIK(signed long PosX,signed long PosZ,signed long PosY,signed int BodyOffsetX,signed int BodyOffsetZ,signed int RotationY){
//Calculating totals from center of the body to the feet
TotalZ = BodyOffsetZ+PosZ;
TotalX = BodyOffsetX+PosX;
//PosY are equal to a "TotalY"
//printf("TotalX=%Ld TotalZ=%Ld PosY=%Ld%c",TotalX,TotalZ,PosY,13);
//Successive global rotation matrix:
//Math shorts for rotation: Alfa (A) = Xrotate, Beta (B) = Zrotate, Gamma (G) = Yrotate
//Sinus Alfa = sinA, cosinus Alfa = cosA. and so on...
//First calculate sinus and cosinus for each rotation:
GetSinCos((float)BodyRotX);
SinG = SinA;
CosG = CosA;
//printf("BodyRotX=%d %c",BodyRotX,13);
//printf("SinG=%11.8f CosG=%11.8f %c",SinG,CosG,13);
GetSinCos((float)BodyRotZ);
SinB = SinA;
CosB = CosA;
//printf("BodyRotZ=%d %c",BodyRotZ,13);
//printf("SinB=%f CosB=%f %c",SinB,CosB,13);
GetSinCos((float)BodyRotY+(float)RotationY);
//printf("BodyRotY=%d RotationY=%d %c",BodyRotY,RotationY,13);
//printf("SinA=%f CosA=%f %c",SinA,CosA,13);
//Calcualtion of rotation matrix:
BodyIKPosX = TotalX-(signed long)Floor((float)TotalX*CosA*CosB-(float)TotalZ*CosB*SinA+(float)PosY*SinB);
Aux = (float)TotalX*CosG*SinA+(float)TotalX*CosA*SinB*SinG;
Aux = Aux+(float)TotalZ*CosA*CosG;
Aux = Aux-(float)TotalZ*SinA*SinB*SinG-(float)PosY*CosB*SinG;
BodyIKPosZ = TotalZ-(signed long)Floor(Aux);
Aux = (float)TotalX*SinA*SinG-(float)TotalX*CosA*CosG*SinB;
//printf("Aux=%f %c",Aux,13);
Aux = Aux+(float)TotalZ*CosA*SinG;
//printf("Aux=%f %c",Aux,13);
Aux = Aux+(float)TotalZ*CosG*SinA*SinB+(float)PosY*CosB*CosG;
//printf("Aux=%f PosY=%Ld %c",Aux,PosY,13);
BodyIKPosY = PosY-(signed long)Floor(Aux);
//printf("BodyIKPosX=%Ld BodyIKPosZ=%Ld BodyIKPosY=%Ld %c",BodyIKPosX,BodyIKPosZ,BodyIKPosY,13);
}
//--------------------------------------------------------------------
//[LEG INVERSE KINEMATICS] Calculates the angles of the tibia and femur for the given position of the feet
//IKFeetPosX - Input position of the Feet X
//IKFeetPosY - Input position of the Feet Y
//IKFeetPosZ - Input Position of the Feet Z
//IKSolution - Output true IF the solution is possible
//IKSolutionWarning - Output true IF the solution is NEARLY possible
//IKSolutionError - Output true IF the solution is NOT possible
//IKFemurAngle - Output Angle of Femur in degrees
//IKTibiaAngle - Output Angle of Tibia in degrees
//IKCoxaAngle - Output Angle of Coxa in degrees
void LegIK(signed long IKFeetPosX, signed long IKFeetPosY, signed long IKFeetPosZ){
float IKSW, IKA1, IKA2;
signed long IKFeetPosXZ;
//printf("IKFeetPosX=%Ld IKFeetPosY=%Ld IKFeetPosZ=%Ld %c",IKFeetPosX,IKFeetPosY,IKFeetPosZ,13);
//Length between the Coxa and Feet
IKFeetPosXZ = (signed long)sqrt(((float)IKFeetPosX*(float)IKFeetPosX)+((float)IKFeetPosZ*(float)IKFeetPosZ));
//printf("IKFeetPosX=%Ld IKFeetPosZ=%Ld %c",IKFeetPosX,IKFeetPosZ,13);
//printf("IKFeetPosXZ %Ld %c",IKFeetPosXZ,13);
//IKSW - Length between shoulder and wrist
IKSW = sqrt((((float)IKFeetPosXZ-(float)CoxaLength)*((float)IKFeetPosXZ-(float)CoxaLength))+((float)IKFeetPosY*(float)IKFeetPosY));
//printf("IKFeetPosXZ=%Ld IKFeetPosY=%Ld %c",IKFeetPosXZ,IKFeetPosY,13);
//printf("IKSW %f",IKSW);
//IKA1 - Angle between SW line and the ground in rad
GetBoogTan(IKFeetPosXZ-CoxaLength, IKFeetPosY);
IKA1 = BoogTan;
//printf("IKA1 %10.5f %c",IKA1,13);
//IKA2 - ?
IKA2 = acos((((float)FemurLength*(float)FemurLength) - ((float)TibiaLength*(float)TibiaLength) + (IKSW*IKSW)) / ((2*(float)Femurlength) * IKSW));
//printf("IKA2 %f %c",IKA2,13);
//IKFemurAngle
IKFemurAngle = ((signed long)Floor(((IKA1 + IKA2) * 180.0) / 3.141592)*-1)+90;
//printf("IKFemurAngle %Ld %c",IKFemurAngle,13);
//IKTibiaAngle
IKTibiaAngle = (90-(signed long)Floor(((acos(((((float)FemurLength*(float)FemurLength) + ((float)TibiaLength*(float)TibiaLength)) - (IKSW*IKSW)) / (2*(float)Femurlength*(float)TibiaLength)))*180.0) / 3.141592)) * -1;
//printf("IKTibiaAngle %Ld %c",IKTibiaAngle,13);
//IKCoxaAngle
GetBoogTan(IKFeetPosZ, IKFeetPosX);
IKCoxaAngle = (signed long)Floor((BoogTan*180.0) / 3.141592);
//Set the Solution quality
if(IKSW < (FemurLength+TibiaLength-30)){
IKSolution = TRUE;
//printf("IKSolution=%f %c",IKSW,13);
}
else{
if (IKSW < (FemurLength+TibiaLength)){
IKSolutionWarning = TRUE;
//printf("IKSolutionWarning=%f %c",IKSW,13);
}
else{
//printf("IKSolutionError=%f %c",IKSW,13);
IKSolutionError = TRUE;
}
}
}
//--------------------------------------------------------------------
//[CHECK ANGLES] Checks the mechanical limits of the servos
void CheckAngles(void){
if (RFCoxaAngle > RFCoxa_MAX) RFCoxaAngle = RFCoxa_MAX;
if (RFCoxaAngle < RFCoxa_MIN) RFCoxaAngle = RFCoxa_MIN;
if (RFFemurAngle > RFFemur_MAX) RFFemurAngle = RFFemur_MAX;
if (RFFemurAngle < RFFemur_MIN) RFFemurAngle = RFFemur_MIN;
if (RFTibiaAngle > RFTibia_MAX) RFTibiaAngle = RFTibia_MAX;
if (RFTibiaAngle < RFTibia_MIN) RFTibiaAngle = RFTibia_MIN;
//Convertir Angulos en tiempo de pulso
RFCoxaAngle = ((RFCoxaAngle+90)/0.10588238)+650;
RFFemurAngle = ((RFFemurAngle+90)/0.10588238)+650;
RFTibiaAngle = ((RFTibiaAngle+90)/0.10588238)+650;
if (RMCoxaAngle > RMCoxa_MAX) RMCoxaAngle = RMCoxa_MAX;
if (RMCoxaAngle < RMCoxa_MIN) RMCoxaAngle = RMCoxa_MIN;
if (RMFemurAngle > RMFemur_MAX) RMFemurAngle = RMFemur_MAX;
if (RMFemurAngle < RMFemur_MIN) RMFemurAngle = RMFemur_MIN;
if (RMTibiaAngle > RMTibia_MAX) RMTibiaAngle = RMTibia_MAX;
if (RMTibiaAngle < RMTibia_MIN) RMTibiaAngle = RMTibia_MIN;
//Convertir Angulos en tiempo de pulso
RMCoxaAngle = ((RMCoxaAngle+90)/0.10588238)+650;
RMFemurAngle = ((RMFemurAngle+90)/0.10588238)+650;
RMTibiaAngle = ((RMTibiaAngle+90)/0.10588238)+650;
if (RRCoxaAngle > RRCoxa_MAX) RRCoxaAngle = RRCoxa_MAX;
if (RRCoxaAngle < RRCoxa_MIN) RRCoxaAngle = RRCoxa_MIN;
if (RRFemurAngle > RRFemur_MAX) RRFemurAngle = RRFemur_MAX;
if (RRFemurAngle < RRFemur_MIN) RRFemurAngle = RRFemur_MIN;
if (RRTibiaAngle > RRTibia_MAX) RRTibiaAngle = RRTibia_MAX;
if (RRTibiaAngle < RRTibia_MIN) RRTibiaAngle = RRTibia_MIN;
//Convertir Angulos en tiempo de pulso
RRCoxaAngle = ((RRCoxaAngle+90)/0.10588238)+650;
RRFemurAngle = ((RRFemurAngle+90)/0.10588238)+650;
RRTibiaAngle = ((RRTibiaAngle+90)/0.10588238)+650;
if (LFCoxaAngle > LFCoxa_MAX) LFCoxaAngle = LFCoxa_MAX;
if (LFCoxaAngle < LFCoxa_MIN) LFCoxaAngle = LFCoxa_MIN;
if (LFFemurAngle > LFFemur_MAX) LFFemurAngle = LFFemur_MAX;
if (LFFemurAngle < LFFemur_MIN) LFFemurAngle = LFFemur_MIN;
if (LFTibiaAngle > LFTibia_MAX) LFTibiaAngle = LFTibia_MAX;
if (LFTibiaAngle < LFTibia_MIN) LFTibiaAngle = LFTibia_MIN;
//Convertir Angulos en tiempo de pulso
LFCoxaAngle = ((-LFCoxaAngle+90)/0.10588238)+650;
LFFemurAngle = ((-LFFemurAngle+90)/0.10588238)+650;
LFTibiaAngle = ((-LFTibiaAngle+90)/0.10588238)+650;
if (LMCoxaAngle > LMCoxa_MAX) LMCoxaAngle = LMCoxa_MAX;
if (LMCoxaAngle < LMCoxa_MIN) LMCoxaAngle = LMCoxa_MIN;
if (LMFemurAngle > LMFemur_MAX) LMFemurAngle = LMFemur_MAX;
if (LMFemurAngle < LMFemur_MIN) LMFemurAngle = LMFemur_MIN;
if (LMTibiaAngle > LMTibia_MAX) LMTibiaAngle = LMTibia_MAX;
if (LMTibiaAngle < LMTibia_MIN) LMTibiaAngle = LMTibia_MIN;
//Convertir Angulos en tiempo de pulso
LMCoxaAngle = ((-LMCoxaAngle+90)/0.10588238)+650;
LMFemurAngle = ((-LMFemurAngle+90)/0.10588238)+650;
LMTibiaAngle = ((-LMTibiaAngle+90)/0.10588238)+650;
if (LRCoxaAngle > LRCoxa_MAX) LRCoxaAngle = LRCoxa_MAX;
if (LRCoxaAngle < LRCoxa_MIN) LRCoxaAngle = LRCoxa_MIN;
if (LRFemurAngle > LRFemur_MAX) LRFemurAngle = LRFemur_MAX;
if (LRFemurAngle < LRFemur_MIN) LRFemurAngle = LRFemur_MIN;
if (LRTibiaAngle > LRTibia_MAX) LRTibiaAngle = LRTibia_MAX;
if (LRTibiaAngle < LRTibia_MIN) LRTibiaAngle = LRTibia_MIN;
//Convertir Angulos en tiempo de pulso
LRCoxaAngle = ((-LRCoxaAngle+90)/0.10588238)+650;
LRFemurAngle = ((-LRFemurAngle+90)/0.10588238)+650;
LRTibiaAngle = ((-LRTibiaAngle+90)/0.10588238)+650;
}
//--------------------------------------------------------------------
//[SERVO DRIVER] Updates the positions of the servos
void ServoDriver(void){
//long contador;
//contador = counter;
//printf("counter=%Ld %c",contador,13);
if(SetupMode){
Return;
}
//Front Right leg
printf("#%d P%Ld ",RFCoxaPin,RFCoxaAngle);
printf("#%d P%Ld ",RFFemurPin,RFFemurAngle);
printf("#%d P%Ld ",RFTibiaPin,RFTibiaAngle);
//Middle Right leg
printf("#%d P%Ld ",RMCoxaPin,RMCoxaAngle);
printf("#%d P%Ld ",RMFemurPin,RMFemurAngle);
printf("#%d P%Ld ",RMTibiaPin,RMTibiaAngle);
//Rear Right leg
printf("#%d P%Ld ",RRCoxaPin,RRCoxaAngle);
printf("#%d P%Ld ",RRFemurPin,RRFemurAngle);
printf("#%d P%Ld ",RRTibiaPin,RRTibiaAngle);
//Front Left leg
printf("#%d P%Ld ",LFCoxaPin,LFCoxaAngle);
printf("#%d P%Ld ",LFFemurPin,LFFemurAngle);
printf("#%d P%Ld ",LFTibiaPin,LFTibiaAngle);
//Middle Left leg
printf("#%d P%Ld ",LMCoxaPin,LMCoxaAngle);
printf("#%d P%Ld ",LMFemurPin,LMFemurAngle);
printf("#%d P%Ld ",LMTibiaPin,LMTibiaAngle);
//Rear Left leg
printf("#%d P%Ld ",LRCoxaPin,LRCoxaAngle);
printf("#%d P%Ld ",LRFemurPin,LRFemurAngle);
printf("#%d P%Ld ",LRTibiaPin,LRTibiaAngle);
//Send <CR>
printf(" T%Ld%c",SSCTime,13);
PrevSSCTime = SSCTime;
}
//--------------------------------------------------------------------
//ServoSetup
void ServoSetup(void){
for (Index=0;Index<=31;Index++){
printf("#%d P1500 ",Index);
}
printf(" T200%c",13);
}
//Setup
void Setup(void){
char Res[5];
short Neg;
int Val,Val1,Val2,Val3,I,c,d;
RegNo=32;
//while(){
for (RegNo=0;RegNo<=31;RegNo++){
d=0;
for(c=0;c<=4;c++){
Res[c]='.';
}
printf("R%u%c",(RegNo+32),13);
Neg=0;
for(c=0;c<=4;c++){
Res[c]=getch();
if((Res[c]=='-')&(c==0)){
Neg=1;
}
if(Res[c]==13){
break;
}
}
Val1=255;
Val2=255;
Val3=255;
d=c-1;
for(i=0;i<=d;i++){
if(Neg&i==0){
continue;
}
switch(Res){
case '0':Val=0;
break;
case '1':Val=1;
break;
case '2':Val=2;
break;
case '3':Val=3;
break;
case '4':Val=4;
break;
case '5':Val=5;
break;
case '6':Val=6;
break;
case '7':Val=7;
break;
case '8':Val=8;
break;
case '9':Val=9;
break;
}
//printf("Val=%d%c",Val,13);
if(Neg){
switch(I){
case 1:Val1=Val;
break;
case 2:Val2=Val;
break;
case 3:Val3=Val;
break;
}
}
else{
switch(I){
case 0:Val1=Val;
break;
case 1:Val2=Val;
break;
case 2:Val3=Val;
break;
}
}
}
//printf("Val1=%u Val2=%u Val3=%u%c",Val1,Val2,Val3,13);
if(Val2==255&Val3==255){
ValReg[RegNo]=(signed int)Val1;
}
else{
if(Val3==255)
ValReg[RegNo]=(signed int)(Val1*10+Val2);
else
ValReg[RegNo]=(signed int)(Val1*100+Val2*10+Val3);
}
if(Neg){
ValReg[RegNo]=ValReg[RegNo]*-1;
}
printf("ValReg[%u]=%d%c",(RegNo+32),ValReg[RegNo],13);
//delay_ms(10);
}
//return;
RegNo=0;
while (SetupMode){
Ps2Input();
printf("#%u PO %d%c",RegNo,ValReg[RegNo],13);
printf("R%u = %d%c",(RegNo+32),ValReg[RegNo],13);
delay_ms(100);
}
}
