Another great quadruped

[innerbreed]

youtube.com/watch?v=Q2YAZHRZ … re=related

you can see how the body is moving around its center of gravity. very cool. 8)

[xan]

The quad really moves very smooth. I would love to try and adapt my code on a quad one day. But the way it looks, you’re gonna beat me to it

Xan

[innerbreed]

with so many Great hexapods and “hex” codes i feel LM is lacking Quadrupeds. id like to bring the developments of quads and their code forward.

i know jim has plans for some work in the quadruped side but not sure if this means just kits or codes as well.

[robotdude]

I am going to try to get some quadruped kits going this year… Full kits with PS2 walking code. It’s so long overdue…

[innerbreed]

YAY!

[likwidchz]

You know honestly I think if Jim sent some servo’s and a boat load of brackets to some people they could come up with a quad kit that would be pretty cheap… “while documenting build instructions”

Frankly I had no idea LM offered so many brackets and servo accessories, I’m pretty sure someone could come up with a quad kit pretty easy without actually having to design any new components…

Currently I have an idea I’ll probably start on within the next week or so for the phoenix … It’s a secret

I’d imagine the brackets LM sells are quite cheap to make although they are fantastic quality

[innerbreed]

@innerbreed

http://www.youtube.com/watch?v=Q2YAZHRZwF0&feature=related

you can see how the body is moving around its center of gravity. very cool. 8)

The Example of Dynamixel Evaluation Quadruped with Atmega128

[code]/*

/*

• included files
  */
  #define ENABLE_BIT_DEFINITIONS
  //#include <io.h>
  #include <inttypes.h>
  #include <avr/io.h>
  #include <avr/interrupt.h>
  #include <avr/signal.h>
  #include <math.h>

#include “linalg.hpp”

#define cbi(REG8,BITNUM) REG8 &= ~(_BV(BITNUM))
#define sbi(REG8,BITNUM) REG8 |= _BV(BITNUM)

typedef unsigned char byte;
typedef unsigned int word;
#define ON 1
#define OFF 0
#define _ON 0
#define _OFF 1

//— Control Table Address —
//EEPROM AREA
#define P_MODEL_NUMBER_L 0
#define P_MODOEL_NUMBER_H 1
#define P_VERSION 2
#define P_ID 3
#define P_BAUD_RATE 4
#define P_RETURN_DELAY_TIME 5
#define P_CW_ANGLE_LIMIT_L 6
#define P_CW_ANGLE_LIMIT_H 7
#define P_CCW_ANGLE_LIMIT_L 8
#define P_CCW_ANGLE_LIMIT_H 9
#define P_SYSTEM_DATA2 10
#define P_LIMIT_TEMPERATURE 11
#define P_DOWN_LIMIT_VOLTAGE 12
#define P_UP_LIMIT_VOLTAGE 13
#define P_MAX_TORQUE_L 14
#define P_MAX_TORQUE_H 15
#define P_RETURN_LEVEL 16
#define P_ALARM_LED 17
#define P_ALARM_SHUTDOWN 18
#define P_OPERATING_MODE 19
#define P_DOWN_CALIBRATION_L 20
#define P_DOWN_CALIBRATION_H 21
#define P_UP_CALIBRATION_L 22
#define P_UP_CALIBRATION_H 23

#define P_TORQUE_ENABLE (24)
#define P_LED (25)
#define P_CW_COMPLIANCE_MARGIN (26)
#define P_CCW_COMPLIANCE_MARGIN (27)
#define P_CW_COMPLIANCE_SLOPE (28)
#define P_CCW_COMPLIANCE_SLOPE (29)
#define P_GOAL_POSITION_L (30)
#define P_GOAL_POSITION_H (31)
#define P_GOAL_SPEED_L (32)
#define P_GOAL_SPEED_H (33)
#define P_TORQUE_LIMIT_L (34)
#define P_TORQUE_LIMIT_H (35)
#define P_PRESENT_POSITION_L (36)
#define P_PRESENT_POSITION_H (37)
#define P_PRESENT_SPEED_L (38)
#define P_PRESENT_SPEED_H (39)
#define P_PRESENT_LOAD_L (40)
#define P_PRESENT_LOAD_H (41)
#define P_PRESENT_VOLTAGE (42)
#define P_PRESENT_TEMPERATURE (43)
#define P_REGISTERED_INSTRUCTION (44)
#define P_PAUSE_TIME (45)
#define P_MOVING (46)
#define P_LOCK (47)
#define P_PUNCH_L (48)
#define P_PUNCH_H (49)

//— Instruction —
#define INST_PING 0x01
#define INST_READ 0x02
#define INST_WRITE 0x03
#define INST_REG_WRITE 0x04
#define INST_ACTION 0x05
#define INST_RESET 0x06
#define INST_DIGITAL_RESET 0x07
#define INST_SYSTEM_READ 0x0C
#define INST_SYSTEM_WRITE 0x0D
#define INST_SYNC_WRITE 0x83
#define INST_SYNC_REG_WRITE 0x84

#define CLEAR_BUFFER gbRxBufferReadPointer = gbRxBufferWritePointer
#define DEFAULT_RETURN_PACKET_SIZE 6
#define BROADCASTING_ID 0xfe

#define TxD8 TxD81
#define RxD8 RxD81

//Hardware Dependent Item
#define DEFAULT_BAUD_RATE 34 //57600bps at 16MHz

////// For CM-5
#define RS485_TXD PORTE &= ~_BV(PE3),PORTE |= _BV(PE2) //_485_DIRECTION = 1
#define RS485_RXD PORTE &= ~_BV(PE2),PORTE |= _BV(PE3) //PORT_485_DIRECTION = 0
/*
////// For CM-2
#define RS485_TXD PORTE |= _BV(PE2); //_485_DIRECTION = 1
#define RS485_RXD PORTE &= ~_BV(PE2);//PORT_485_DIRECTION = 0
*/
//#define TXD0_FINISH UCSR0A,6 //This bit is for checking TxD Buffer in CPU is empty or not.
//#define TXD1_FINISH UCSR1A,6

#define SET_TxD0_FINISH sbi(UCSR0A,6)
#define RESET_TXD0_FINISH cbi(UCSR0A,6)
#define CHECK_TXD0_FINISH bit_is_set(UCSR0A,6)
#define SET_TxD1_FINISH sbi(UCSR1A,6)
#define RESET_TXD1_FINISH cbi(UCSR1A,6)
#define CHECK_TXD1_FINISH bit_is_set(UCSR1A,6)

#define RX_INTERRUPT 0x01
#define TX_INTERRUPT 0x02
#define OVERFLOW_INTERRUPT 0x01
#define SERIAL_PORT0 0
#define SERIAL_PORT1 1
#define BIT_RS485_DIRECTION0 0x08 //Port E
#define BIT_RS485_DIRECTION1 0x04 //Port E

#define BIT_ZIGBEE_RESET PD4 //out : default 1 //PORTD
#define BIT_ENABLE_RXD_LINK_PC PD5 //out : default 1
#define BIT_ENABLE_RXD_LINK_ZIGBEE PD6 //out : default 0
#define BIT_LINK_PLUGIN PD7 //in, no pull up

void TxD81(byte bTxdData);
void TxD80(byte bTxdData);
void TxDString(byte* bData);
void TxDString(const char bData) { TxDString(reinterpret_cast<byte>(const_cast<char*>(bData))); }
void TxD8Hex(byte bSentData);
void TxD32Dec(long lLong);
byte RxD81(void);
void MiliSec(word wDelayTime);
void PortInitialize(void);
void SerialInitialize(byte bPort, byte bBaudrate, byte bInterrupt);
byte TxPacket(byte bID, byte bInstruction, byte bParameterLength);
byte RxPacket(byte bRxLength);
void PrintBuffer(byte *bpPrintBuffer, byte bLength);

// — Gloval Variable Number —
volatile byte gbpRxInterruptBuffer[256];
byte gbpParameter[128];
byte gbRxBufferReadPointer;
byte gbpRxBuffer[128];
byte gbpTxBuffer[128];
volatile byte gbRxBufferWritePointer;

union bytes_t { int i; char c[2]; } bytes;

void relax(void)
{
byte bTxPacketLength,bRxPacketLength;

for (int i = 1; i != 20; ++i)
{    
if (i != 13) {
    gbpParameter[0] = P_TORQUE_ENABLE; // Address
    gbpParameter[1] = 0; // Disabled
    bTxPacketLength = TxPacket(i,INST_WRITE,2);
    bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE);         
 }  }

}

#define pi 3.141592654

void set4(double a[4], double b[4])
{
for (int i = 0; i != 4; ++i)
a* = b*;
}

void mul441(double a[4], double b[4][4], double c[4])
{
for (int i = 0; i != 4; ++i)
{
a* = 0;
for (int j = 0; j != 4; ++j)
a* += b*[j] * c[j];
}

}

void mul444(double a[4][4], double b[4][4], double c[4][4])
{
for (int i = 0; i != 4; ++i)
for (int k = 0; k != 4; ++k)
{
a*[k] = 0;
for (int j = 0; j != 4; ++j)
a*[k] += b*[j] * c[j][k];
}
}

void zero44(double a[4][4])
{
for (int i = 0; i != 4; ++i)
for (int j = 0; j != 4; ++j)
a*[j] = 0.0;
}

void identity4(double a[4][4])
{
for (int i = 0; i != 4; ++i)
for (int j = 0; j != 4; ++j)
a*[j] = ((i == j) ? 1.0 : 0.0);
}

void translate(double a[4][4], double b[4])
{
identity4(a);
for (int i = 0; i != 4; ++i)
a*[3] = b*;
}

void rotatex(double a[4][4], double b)
{
identity4(a);
a[1][1] = cos(b);
a[1][2] = sin(b);
a[2][1] = -sin(b);
a[2][2] = cos(b);
}

void rotatey(double a[4][4], double b)
{
identity4(a);
a[0][0] = cos(b);
a[0][2] = sin(b);
a[2][0] = -sin(b);
a[2][2] = cos(b);
}

void rotatez(double a[4][4], double b)
{
identity4(a);
a[0][0] = cos(b);
a[0][1] = sin(b);
a[1][0] = -sin(b);
a[1][1] = cos(b);
}

typedef struct
{
int counts;
double radians;
double a;
double b;
int load;
char present;
} servo_t;

servo_t servos[20];

#define LENGTH_UPPER 72.0
#define LENGTH_LOWER 80.0

void reverse_servo(int i)
{
servos*.a = -servos*.a;
servos*.b += 1023 / servos*.a;
}

void init_servos(void)
{
for (int i = 1; i != 20; ++i)
{
servos*.a = 195.57;
servos*.b = 2.61799;
servos*.present = 1;
}

servos 1].b -= pi / 4;
servos 5].b += pi / 4;
servos 9].b += pi / 4;
servos[13].b -= pi / 4;

reverse_servo(1);
reverse_servo(5);
reverse_servo(9);
reverse_servo(13);

reverse_servo(2);
reverse_servo(4);
reverse_servo(14);
reverse_servo(16);

}

void read_angles(void)
{
byte bTxPacketLength,bRxPacketLength;
for (int i = 1; i != 20; ++i)
{
if (servos*.present)
{

      gbpParameter[0] = P_PRESENT_POSITION_L; //Address of Firmware Version
      gbpParameter[1] = 6; //Read Length
      bTxPacketLength = TxPacket(i,INST_READ,2);
      bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]);
      //TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength);
      //TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength);
      //if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE + gbpParameter[1])
      //{
      //  TxDString("\r\n Return Error      : ");TxD8Hex(gbpRxBuffer[4]);
      //  TxDString("\r\n Firmware Version  : ");TxD8Hex(gbpRxBuffer[5]);
      //}
      bytes.c[0] = gbpRxBuffer[5];
      bytes.c[1] = gbpRxBuffer[6];
      servos*.counts = bytes.i;
      servos*.radians = servos*.counts / servos*.a - servos*.b;
      
      bytes.c[0] = gbpRxBuffer[9];
      bytes.c[1] = gbpRxBuffer[10];
      //if (bytes.i < 512)
      //{
        //servos*.load = bytes.i;
      servos*.load = (bytes.i & 0x400) ? - (bytes.i & 0x3FF) : (bytes.i & 0x3FF);
      //}
      //else
      //{
        //servos*.load = 512 - bytes.i;
      //}
        
      
      //TxD32Dec(i);
      //TxDString(" ");
      //TxD32Dec(bytes.i);
      //TxDString("\r\n");
      }
}    

}

unsigned char ir[3];

void read_sensor(void)
{
byte bTxPacketLength,bRxPacketLength;

gbpParameter[0] = 0x1a; //Address of Firmware Version
gbpParameter[1] = 3; //Read Length
bTxPacketLength = TxPacket(100,INST_READ,2);
bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]);
  //TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength);
  //TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength);
  //if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE + gbpParameter[1])
  //{
  //  TxDString("\r\n Return Error      : ");TxD8Hex(gbpRxBuffer[4]);
  //  TxDString("\r\n Firmware Version  : ");TxD8Hex(gbpRxBuffer[5]);
  //}
ir[0] = gbpRxBuffer[5];
ir[1] = gbpRxBuffer[6];
ir[2] = gbpRxBuffer[7];
//servos*.counts = bytes.i;
//servos*.radians = servos*.counts / servos*.a - servos*.b;
  //TxD32Dec(i);
  //TxDString(" ");
  //TxD32Dec(bytes.i);
  //TxDString("\r\n");

}

void write_angles(void)
{
byte bTxPacketLength,bRxPacketLength;

for (int i = 1; i != 20; ++i)
{    
    if (servos*.present) {
    
    servos*.counts = (servos*.radians + servos*.b) * servos*.a;
    bytes.i = servos*.counts;
    /*
    TxDString("\r\n"); 
    TxD32Dec(i);
    TxDString(" ");
    TxD32Dec(bytes.i);
      */  
    
    
    gbpParameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
    gbpParameter[1] = bytes.c[0]; //Writing Data P_GOAL_POSITION_L
    gbpParameter[2] = bytes.c[1]; //Writing Data P_GOAL_POSITION_H
    gbpParameter[3] = 0xff; //Writing Data P_GOAL_SPEED_L
    gbpParameter[4] = 0x03; //Writing Data P_GOAL_SPEED_H
    gbpParameter[5] = 0xff; //Writing Data P_TORQUE_LIMIT_L
    gbpParameter[6] = 0x03; //Writing Data P_TORQUE_LIMIT_H
    bTxPacketLength = TxPacket(i,INST_WRITE,5);
    bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE); 
    }
    
 }  

}

typedef struct
{
vector<4> mm;

int id[4];
double origin[2];
double angle;
char flag;

} limb_t;

limb_t limbs[4];

void init_limbs(void)
{
limbs[0].id[0] = 1;
limbs[0].id[1] = 2;
limbs[0].id[2] = 3;
limbs[0].id[3] = 4;

limbs[1].id[0] = 5;
limbs[1].id[1] = 6;
limbs[1].id[2] = 7;
limbs[1].id[3] = 8;

limbs[2].id[0] = 9;
limbs[2].id[1] = 10;
limbs[2].id[2] = 11;
limbs[2].id[3] = 12;

limbs[3].id[0] = 13;
limbs[3].id[1] = 14;
limbs[3].id[2] = 15;
limbs[3].id[3] = 16;

limbs[0].origin[0] = +62;
limbs[0].origin[1] = +56;

limbs[1].origin[0] = +62;
limbs[1].origin[1] = -56;

limbs[2].origin[0] = -62;
limbs[2].origin[1] = +56;

limbs[3].origin[0] = -62;
limbs[3].origin[1] = -56;

limbs[0].angle = +    pi / 4;
limbs[1].angle = -    pi / 4;
limbs[2].angle = +3 * pi / 4;
limbs[3].angle = -3 * pi / 4;

limbs[0].mm = make_vector(+100, +100, 0, 1);
limbs[1].mm = make_vector(+100, -100, 0, 1);
limbs[2].mm = make_vector(-100, +100, 0, 1);
limbs[3].mm = make_vector(-100, 100, 0, 1);

limbs[0].flag = 1;
limbs[1].flag = 1;
limbs[2].flag = 1;
limbs[3].flag = 1;

}

void inverse_kinematics(void)
{
for (int i = 0; i != 4; ++i)
{
double x = limbs*.mm[0] - limbs*.origin[0];
double y = limbs*.mm[1] - limbs*.origin[1];
double a = atan2(y, x);
double s = sqrt(xx + yy);
double z = limbs*.mm[2];
double b = atan(z / s);
double r2 = xx + yy + z*z;
double c = acos((LENGTH_UPPER * LENGTH_UPPER + LENGTH_LOWER * LENGTH_LOWER - r2) / (2 * LENGTH_UPPER * LENGTH_LOWER));
double d = acos((LENGTH_UPPER * LENGTH_UPPER - LENGTH_LOWER * LENGTH_LOWER + r2) / (2 * LENGTH_UPPER * sqrt(r2)));

    a -= limbs*.angle;
    
    if ((a > -pi/3) && (a < pi/3) && (r2 < ((LENGTH_UPPER + LENGTH_LOWER)*(LENGTH_UPPER + LENGTH_LOWER))))
    {        
        servos[limbs[i].id[0]].radians = a;
        servos[limbs[i].id[1]].radians = d - b;
        servos[limbs[i].id[3]].radians = pi - c;
        limbs*.flag = 0;
        
    }
    else
    {
        limbs*.flag = 1;
        TxDString("\r\n Can't reach with limb "); TxD32Dec(i);
        while (1);
    }
}

}

void forward_kinematics(void)
{
for (int i = 0; i != 4; ++i)
{
double b = servos[limbs[i].id[1]].radians;
double c = servos[limbs[i].id[3]].radians;
limbs*.mm[2] = LENGTH_UPPER * sin(-b) + LENGTH_LOWER * sin(-b + c);
double s = LENGTH_UPPER * cos(-b) + LENGTH_LOWER * cos(-b + c);
double a = servos[limbs[i].id[0]].radians + limbs*.angle;
limbs*.mm[0] = s * cos(a) + limbs*.origin[0];
limbs*.mm[1] = s * sin(a) + limbs*.origin[1];
}
}

#define max(A, B) (((A) > (B)) ? (A) : (B))
#define min(A, B) (((A) < (B)) ? (A) : (B))

double step_v(double x)
{
if ((x <= -0.5) || (x >= 0.5))
{
return 0.0;
}
else
{
double y = (1.0 - 4 * x * x);
return y * y;
}
}

double step_u(double x)
{
if (x <= -0.5)
{
return -2./3x-4./3;
}
else
{
if (x >= 0.5)
{
return -2./3x+4./3;
}
else
{
return x * (x * x * (-16./3) + (10./3));
}
}
}

void user(void)
{
init_servos();
init_limbs();

read_angles();

forward_kinematics();

double t = 0;

/*
do
{
    read_sensor();
} 
while ((ir[0] >= 200) && (ir[1] >= 200) && (ir[2] >= 200));   

do
{
    read_sensor();
} while ((ir[0] < 200) || (ir[1] < 200) || (ir[2] < 200));
*/

//double target[4][4] = { 
//    {  150,  150, 0, 1 },
//    {  150, -150, 0, 1 },
//    { -150,  150, 0, 1 },
//    { -150, -150, 0, 1 } };   
while (RxD8() != 'p');


//TxDString("\r\n");


int count = 0;



while(1)
{

/*
do
{
read_sensor();
}
while ((ir[0] >= 200) && (ir[1] >= 200) && (ir[2] >= 200));
*/

    double stride = 40; // 40;
    double lean   = 20; // 20;
    double lift   = 40; // 40

    limbs[0].mm[0] = +120 + lean * cos(t * pi / 2);
    limbs[1].mm[0] = +120 + lean * cos(t * pi / 2);
    limbs[2].mm[0] = -120 + lean * cos(t * pi / 2);
    limbs[3].mm[0] = -120 + lean * cos(t * pi / 2);

    limbs[0].mm[1] = +120 + lean * sin(t * pi) + stride * step_u(t - 0.5);
    limbs[1].mm[1] = -120 + lean * sin(t * pi) + stride * step_u(t - 3.5);
    limbs[2].mm[1] = +120 + lean * sin(t * pi) + stride * step_u(t - 2.5);
    limbs[3].mm[1] = -120 + lean * sin(t * pi) + stride * step_u(t - 1.5);
    
    limbs[0].mm[2] = +80 - lift * step_v(t - 0.5);
    limbs[1].mm[2] = +80 - lift * step_v(t - 3.5);
    limbs[2].mm[2] = +80 - lift * step_v(t - 2.5);
    limbs[3].mm[2] = +80 - lift * step_v(t - 1.5);  
    
    //servos[14].radians = 14.0 / LENGTH_NECK * sin(t * pi);
    //servos[15].radians = -14.0 / LENGTH_NECK * sin(t * pi);
  
    inverse_kinematics();        
            
    write_angles();
    
    t += 0.015; if (t >= 4) t -= 4;
   
}


while(1)
{

    int oldir = ir[1];

    read_sensor();
    
    if (oldir == ir[1])
    {
        ++count;
       
    }
    else
    {
        TxD32Dec(count);
        TxDString("\r\n");         
        count = 0;
    }
    

    
    
   /*
    double d = .3;
   
    if ((t < 50) && (t > -50))
    {
        if (ir[1] < 64)
        {
            if (t > 0)
            {
                t -= d;
            }
            else
            {
                t += d;
            }
        }
        else if (ir[1] < 128)
        {
            t -= d;
        }
        else if(ir[1] < 192)
        {
        }
        else
        {
            t += d;
        }
    }
    else
    {
        if (t > 0)
        {
            t -= d;
        }
        else
        {
            t += d;
        }
    }*/
        

    limbs[0].mm[0] = +120;
    limbs[1].mm[0] = +120;
    limbs[2].mm[0] = -120;
    limbs[3].mm[0] = -120;

    limbs[0].mm[1] = +120 + t;
    limbs[1].mm[1] = -120 + t;
    limbs[2].mm[1] = +120 + t;
    limbs[3].mm[1] = -120 + t;
    
    limbs[0].mm[2] = +80;
    limbs[1].mm[2] = +80;
    limbs[2].mm[2] = +80;
    limbs[3].mm[2] = +80;
   
    inverse_kinematics();        
            
    write_angles();       
   
}  

//while(1)
//{

    //read_angles();
    //forward_kinematics();    
    
    //limbs[0].mm = make_vector( 120.,  120., 80., 1.);
    //limbs[1].mm = make_vector( 120., -120., 80., 1.);
    //limbs[2].mm = make_vector(-120.,  120., 80., 1.);
    //limbs[3].mm = make_vector(-120., -120., 80., 1.);
    //inverse_kinematics();
    //write_angles();
    
    //TxD32Dec(limbs[0].mm[0]);
    //TxDString(" ");       
    //TxD32Dec(limbs[0].mm[1]);
    //TxDString(" ");
    //TxD32Dec(limbs[0].mm[2]);
    //TxDString("\r");         
    
    //inverse_kinematics();
    //write_angles();      

    //t += 0.01;
    
//}

/*
while(1)
{

    limbs[0].mm[0] = +120 + 40 * step_u(t - 0.5) + 20 * sin(t * pi);
    limbs[1].mm[0] = +120 + 40 * step_u(t - 2.5) + 20 * sin(t * pi);
    limbs[2].mm[0] = -120 + 40 * step_u(t - 3.5) + 20 * sin(t * pi);
    limbs[3].mm[0] = -120 + 40 * step_u(t - 1.5) + 20 * sin(t * pi);

    limbs[0].mm[1] = +120 + 20.0 * cos(t * pi / 2);
    limbs[1].mm[1] = -120 + 20.0 * cos(t * pi / 2);
    limbs[2].mm[1] = +120 + 20.0 * cos(t * pi / 2);
    limbs[3].mm[1] = -120 + 20.0 * cos(t * pi / 2);
    
    limbs[0].mm[2] = +80 - 40 * step_v(t - 0.5);
    limbs[1].mm[2] = +80 - 40 * step_v(t - 2.5);
    limbs[2].mm[2] = +80 - 40 * step_v(t - 3.5);
    limbs[3].mm[2] = +80 - 40 * step_v(t - 1.5);  
    
    servos[14].radians = 14.0 / LENGTH_NECK * sin(t * pi);
    servos[15].radians = -14.0 / LENGTH_NECK * sin(t * pi);
  
    inverse_kinematics();        
            
    write_angles();
    
    t += 0.014; if (t >= 4) t -= 4;
   
}
*/

       //double a[4][4];
    //double b[4];
    /*double c[4][4];
    double d[4][4];*/

    //t += 0.01;
            
    /*rotatex(a, 0.1 * sin(t));*//*
    b[0] = 30; b[1] = 30; b[2] = 0; b[3] = 0;        
    translate(a, b);*/
    /*mul444(d, a, c);
                           b[2] = 90.0;
    translate(c, b);
    mul444(a, c, d);*/
    /*        
    for (int i = 0; i != 4; ++i)
    {
        mul441(b, a, target*);
        set4(limbs*.mm, b);
    }
    
    limbs[0].mm[2] = 70.0 + 20.0 * cos(t);
    *//*
    TxD32Dec(limbs[0].mm[0]);
    TxDString(" ");       
    TxD32Dec(limbs[0].mm[1]);
    TxDString(" ");
    TxD32Dec(limbs[0].mm[2]);
    TxDString("\r\n");        
    */
    //TxD32Dec(bytes.i);
  //TxDString("\r\n");

/*
for (int i = 1; i != 14; ++i)
{
servos*.radians = 0.0;
}

double t = 0;

while (1)
{        
    t += 0.02;
    
    limbs[0].mm[0] = +100 + 20.0 * sin(t);
    limbs[1].mm[0] = +100 + 20.0 * sin(t);
    limbs[2].mm[0] = -100 + 20.0 * sin(t);
    limbs[3].mm[0] = -100 + 20.0 * sin(t);

    limbs[0].mm[1] = +100 + 20.0 * sin(t * 0.7);
    limbs[1].mm[1] = -100 + 20.0 * sin(t * 0.7);
    limbs[2].mm[1] = +100 + 20.0 * sin(t * 0.7);
    limbs[3].mm[1] = -100 + 20.0 * sin(t * 0.7);        
    
    inverse_kinematics();
    
    servos[13].radians = sin(t * 0.6) * 0.3;

    write_angles();
}
*/

}

int main(void)
{
byte bCount,bID, bTxPacketLength,bRxPacketLength;

PortInitialize(); //Port In/Out Direction Definition
RS485_RXD; //Set RS485 Direction to Input State.
SerialInitialize(SERIAL_PORT0,1,RX_INTERRUPT);//RS485 Initializing(RxInterrupt)
SerialInitialize(SERIAL_PORT1,DEFAULT_BAUD_RATE,0); //RS232 Initializing(None Interrupt)

gbRxBufferReadPointer = gbRxBufferWritePointer = 0; //RS485 RxBuffer Clearing.

sei(); //Enable Interrupt – Compiler Function

// Beginning of ACS edits

TxDString("\r\nPress [p] to begin:");

relax(); /* a reset will make the robot go limp */

//while (RxD8() != ‘p’);

TxDString("\r\nRunning…\r\n");

double t = 0;

#define PI 3.141592654

user();

return 0;

while (1)
{

double x =  64.5 + 20.0*sin(t);
double y =   0.0 + 20.0*sin(t);
double z = 92.0 + 20.0*cos(t);

double r = sqrt(x*x + y*y);
double s = sqrt(x*x + y*y + z*z);

double a = atan2(y, x);
double b = atan2(r, z);
double c = acos((64.5*64.5 + s*s - 92*92) / (2 * 64.5 * s));
double d = acos((64.5*64.5 + 92*92 - s*s) / (2*64.5 * 92));

c = b + c - 1.20474;
d = (PI - d) + 0.366057 + 0.0434509;

if (a > PI/2) a = PI/2;
if (a < -PI/2) a = -PI/2;
if (b > PI/2) b = PI/2;
if (b < -PI/2) b = -PI/2;  
if (c > PI/2) c = PI/2;
if (c < -PI/2) c = -PI/2;   

bytes.i = 512.0 + (512.0 / 150.0) * (180.0 / PI) * a;

gbpParameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
gbpParameter[1] = bytes.c[0]; //Writing Data P_GOAL_POSITION_L
gbpParameter[2] = bytes.c[1]; //Writing Data P_GOAL_POSITION_H
gbpParameter[3] = 0xff; //Writing Data P_GOAL_SPEED_L
gbpParameter[4] = 0x00; //Writing Data P_GOAL_SPEED_H
bTxPacketLength = TxPacket(1,INST_WRITE,5);
bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE);

bytes.i = 512.0 + (512.0 / 150.0) * (180.0 / PI) * c;

gbpParameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
gbpParameter[1] = bytes.c[0]; //Writing Data P_GOAL_POSITION_L
gbpParameter[2] = bytes.c[1]; //Writing Data P_GOAL_POSITION_H
gbpParameter[3] = 0xff; //Writing Data P_GOAL_SPEED_L
gbpParameter[4] = 0x00; //Writing Data P_GOAL_SPEED_H
bTxPacketLength = TxPacket(2,INST_WRITE,5);
bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE);


bytes.i = 512.0 + (512.0 / 150.0) * (180.0 / PI) * d;

gbpParameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
gbpParameter[1] = bytes.c[0]; //Writing Data P_GOAL_POSITION_L
gbpParameter[2] = bytes.c[1]; //Writing Data P_GOAL_POSITION_H
gbpParameter[3] = 0xff; //Writing Data P_GOAL_SPEED_L
gbpParameter[4] = 0x00; //Writing Data P_GOAL_SPEED_H
bTxPacketLength = TxPacket(3,INST_WRITE,5);
bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE);


t += 0.01;

}

/*char* p;
p = 32;
TxD32Dec(*p);
TxDString("\r]\n");*/

/*
t += 0.01;
bytes.i = 512.0 + 64.0 * sin(t);

  gbpParameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
  gbpParameter[1] = bytes.c[0]; //Writing Data P_GOAL_POSITION_L
  gbpParameter[2] = bytes.c[1]; //Writing Data P_GOAL_POSITION_H
  gbpParameter[3] = 0xff; //Writing Data P_GOAL_SPEED_L
  gbpParameter[4] = 0x03; //Writing Data P_GOAL_SPEED_H
  bTxPacketLength = TxPacket(1,INST_WRITE,5);
  bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE);
  
  */
  /*TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength);
  TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength);	*/

//}

// End of ACS edits

#if 0

TxDString("\r\n [The Example of Dynamixel Evaluation with ATmega128,GCC-AVR]");

//Dynamixel Communication Function Execution Step.
// Step 1. Parameter Setting (gbpParameter]). In case of no parameter instruction(Ex. INST_PING), this step is not needed.
// Step 2. TxPacket(ID,INSTRUCTION,LengthOfParameter); --Total TxPacket Length is returned
// Step 3. RxPacket(ExpectedReturnPacketLength); – Real RxPacket Length is returned
// Step 4 PrintBuffer(BufferStartPointer,LengthForPrinting);

bID = 1;
TxDString("\r\n\n Example 1. Scanning Dynamixels(0~9). – Any Key to Continue."); RxD8();
for(bCount = 0; bCount < 0x0A; bCount++)
{
bTxPacketLength = TxPacket(bCount,INST_PING,0);
bRxPacketLength = RxPacket(255);
TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength);
TxDString(", RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength);
if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE)
{
TxDString(" Found!! ID:");TxD8Hex(bCount);
bID = bCount;
}
}

TxDString("\r\n\n Example 2. Read Firmware Version. – Any Key to Continue."); RxD8();
gbpParameter[0] = P_VERSION; //Address of Firmware Version
gbpParameter[1] = 1; //Read Length
bTxPacketLength = TxPacket(bID,INST_READ,2);
bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]);
TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength);
TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength);
if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1])
{
TxDString("\r\n Return Error : “);TxD8Hex(gbpRxBuffer[4]);
TxDString(”\r\n Firmware Version : ");TxD8Hex(gbpRxBuffer[5]);
}

TxDString("\r\n\n Example 3. LED ON – Any Key to Continue."); RxD8();
gbpParameter[0] = P_LED; //Address of LED
gbpParameter[1] = 1; //Writing Data
bTxPacketLength = TxPacket(bID,INST_WRITE,2);
bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE);
TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength);
TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength);

TxDString("\r\n\n Example 4. LED OFF – Any Key to Continue."); RxD8();
gbpParameter[0] = P_LED; //Address of LED
gbpParameter[1] = 0; //Writing Data
bTxPacketLength = TxPacket(bID,INST_WRITE,2);
bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE);
TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength);
TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength);

TxDString("\r\n\n Example 5. Read Control Table. – Any Key to Continue."); RxD8();
gbpParameter[0] = 0; //Reading Address
gbpParameter[1] = 49; //Read Length
bTxPacketLength = TxPacket(bID,INST_READ,2);
bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]);

TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength);
TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength);
if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1])
{
TxDString("\r\n");
for(bCount = 0; bCount < 49; bCount++)
{
TxD8(’’);TxD8Hex(bCount);TxDString("]:"); TxD8Hex(gbpRxBuffer[bCount+5]);TxD8(’ ');
}
}

TxDString("\r\n\n Example 6. Go 0x200 with Speed 0x100 – Any Key to Continue."); RxD8();
gbpParameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
gbpParameter[1] = 0x00; //Writing Data P_GOAL_POSITION_L
gbpParameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H
gbpParameter[3] = 0x00; //Writing Data P_GOAL_SPEED_L
gbpParameter[4] = 0x01; //Writing Data P_GOAL_SPEED_H
bTxPacketLength = TxPacket(bID,INST_WRITE,5);
bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE);
TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength);
TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength);

TxDString("\r\n\n Example 7. Go 0x00 with Speed 0x40 – Any Key to Continue."); RxD8();
gbpParameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
gbpParameter[1] = 0x00; //Writing Data P_GOAL_POSITION_L
gbpParameter[2] = 0x00; //Writing Data P_GOAL_POSITION_H
gbpParameter[3] = 0x40; //Writing Data P_GOAL_SPEED_L
gbpParameter[4] = 0x00; //Writing Data P_GOAL_SPEED_H
bTxPacketLength = TxPacket(bID,INST_WRITE,5);
bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE);
TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength);
TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength);

TxDString("\r\n\n Example 8. Go 0x3ff with Speed 0x3ff – Any Key to Continue."); RxD8();
gbpParameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version
gbpParameter[1] = 0xff; //Writing Data P_GOAL_POSITION_L
gbpParameter[2] = 0x03; //Writing Data P_GOAL_POSITION_H
gbpParameter[3] = 0xff; //Writing Data P_GOAL_SPEED_L
gbpParameter[4] = 0x03; //Writing Data P_GOAL_SPEED_H
bTxPacketLength = TxPacket(bID,INST_WRITE,5);
bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE);
TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength);
TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength);

TxDString("\r\n\n Example 9. Torque Off – Any Key to Continue."); RxD8();
gbpParameter[0] = P_TORQUE_ENABLE; //Address of LED
gbpParameter[1] = 0; //Writing Data
bTxPacketLength = TxPacket(bID,INST_WRITE,2);
bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE);
TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength);
TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength);

TxDString("\r\n\n End. Push reset button for repeat");

#endif

TxDString("\r\n\nEnd. Push MODE button to restart");
while(1);
}

void PortInitialize(void)
{
DDRA = DDRB = DDRC = DDRD = DDRE = DDRF = 0; //Set all port to input direction first.
PORTB = PORTC = PORTD = PORTE = PORTF = PORTG = 0x00; //PortData initialize to 0
cbi(SFIOR,2); //All Port Pull Up ready
DDRE |= (BIT_RS485_DIRECTION0|BIT_RS485_DIRECTION1); //set output the bit RS485direction

DDRD |= (BIT_ZIGBEE_RESET|BIT_ENABLE_RXD_LINK_PC|BIT_ENABLE_RXD_LINK_ZIGBEE);

PORTD &= ~_BV(BIT_LINK_PLUGIN); // no pull up
PORTD |= _BV(BIT_ZIGBEE_RESET);
PORTD |= _BV(BIT_ENABLE_RXD_LINK_PC);
PORTD |= _BV(BIT_ENABLE_RXD_LINK_ZIGBEE);
}

/*
TxPacket() send data to RS485.
TxPacket() needs 3 parameter; ID of Dynamixel, Instruction byte, Length of parameters.
TxPacket() return length of Return packet from Dynamixel.
*/
byte TxPacket(byte bID, byte bInstruction, byte bParameterLength)
{
byte bCount,bCheckSum,bPacketLength;

gbpTxBuffer[0] = 0xff;
gbpTxBuffer[1] = 0xff;
gbpTxBuffer[2] = bID;
gbpTxBuffer[3] = bParameterLength+2; //Length(Paramter,Instruction,Checksum)
gbpTxBuffer[4] = bInstruction;
for(bCount = 0; bCount < bParameterLength; bCount++)
{
    gbpTxBuffer[bCount+5] = gbpParameter[bCount];
}
bCheckSum = 0;
bPacketLength = bParameterLength+4+2;
for(bCount = 2; bCount < bPacketLength-1; bCount++) //except 0xff,checksum
{
    bCheckSum += gbpTxBuffer[bCount];
}
gbpTxBuffer[bCount] = ~bCheckSum; //Writing Checksum with Bit Inversion

RS485_TXD;
for(bCount = 0; bCount < bPacketLength; bCount++)
{
    sbi(UCSR0A,6);//SET_TXD0_FINISH;
    TxD80(gbpTxBuffer[bCount]);
}
while(!CHECK_TXD0_FINISH); //Wait until TXD Shift register empty
RS485_RXD;
return(bPacketLength);

}

/*
RxPacket() read data from buffer.
RxPacket() need a Parameter; Total length of Return Packet.
RxPacket() return Length of Return Packet.
*/

byte RxPacket(byte bRxPacketLength)
{
#define RX_TIMEOUT_COUNT2 3000L
#define RX_TIMEOUT_COUNT1 (RX_TIMEOUT_COUNT2*10L)
unsigned long ulCounter;
byte bCount, bLength, bChecksum;
byte bTimeout;

bTimeout = 0;
for(bCount = 0; bCount < bRxPacketLength; bCount++)
{
ulCounter = 0;
while(gbRxBufferReadPointer == gbRxBufferWritePointer)
{
if(ulCounter++ > RX_TIMEOUT_COUNT1)
{
bTimeout = 1;
break;
}
}
if(bTimeout) break;
gbpRxBuffer[bCount] = gbpRxInterruptBuffer[gbRxBufferReadPointer++];
}
bLength = bCount;
bChecksum = 0;

if(gbpTxBuffer[2] != BROADCASTING_ID)
{
if(bTimeout && bRxPacketLength != 255)
{
TxDString("\r\n [Error:RxD Timeout]");
CLEAR_BUFFER;
}

if(bLength > 3) //checking is available.
{
  if(gbpRxBuffer[0] != 0xff || gbpRxBuffer[1] != 0xff ) 
  {
    TxDString("\r\n [Error:Wrong Header]");
    CLEAR_BUFFER;
    return 0;
  }
  if(gbpRxBuffer[2] != gbpTxBuffer[2] )
  {
    TxDString("\r\n [Error:TxID != RxID]");
    CLEAR_BUFFER;
    return 0;
  }  
  if(gbpRxBuffer[3] != bLength-4) 
  {
    TxDString("\r\n [Error:Wrong Length]");
    CLEAR_BUFFER;
    return 0;
  }  
  for(bCount = 2; bCount < bLength; bCount++) bChecksum += gbpRxBuffer[bCount];
  if(bChecksum != 0xff) 
  {
    TxDString("\r\n [Error:Wrong CheckSum]");
    CLEAR_BUFFER;
    return 0;
  }
}

}
return bLength;
}

/*
PrintBuffer() print data in Hex code.
PrintBuffer() needs two parameter; name of Pointer(gbpTxBuffer, gbpRxBuffer)
*/
void PrintBuffer(byte *bpPrintBuffer, byte bLength)
{
byte bCount;
for(bCount = 0; bCount < bLength; bCount++)
{
TxD8Hex(bpPrintBuffer[bCount]);
TxD8(’ ‘);
}
TxDString("(LEN:");TxD8Hex(bLength);TxD8(’)’);
}

/*
Print value of Baud Rate.
*/
void PrintBaudrate(void)
{
TxDString("\r\n RS232:");TxD32Dec((16000000L/8L)/((long)UBRR1L+1L) ); TxDString(" BPS,");
TxDString(" RS485:");TxD32Dec((16000000L/8L)/((long)UBRR0L+1L) ); TxDString(" BPS");
}

/Hardware Dependent Item/
#define TXD1_READY bit_is_set(UCSR1A,5) //(UCSR1A_Bit5)
#define TXD1_DATA (UDR1)
#define RXD1_READY bit_is_set(UCSR1A,7)
#define RXD1_DATA (UDR1)

#define TXD0_READY bit_is_set(UCSR0A,5)
#define TXD0_DATA (UDR0)
#define RXD0_READY bit_is_set(UCSR0A,7)
#define RXD0_DATA (UDR0)

/*
SerialInitialize() set Serial Port to initial state.
Vide Mega128 Data sheet about Setting bit of register.
SerialInitialize() needs port, Baud rate, Interrupt value.

*/
void SerialInitialize(byte bPort, byte bBaudrate, byte bInterrupt)
{
if(bPort == SERIAL_PORT0)
{
UBRR0H = 0; UBRR0L = bBaudrate;
UCSR0A = 0x02; UCSR0B = 0x18;
if(bInterrupt&RX_INTERRUPT) sbi(UCSR0B,7); // RxD interrupt enable
UCSR0C = 0x06; UDR0 = 0xFF;
sbi(UCSR0A,6);//SET_TXD0_FINISH; // Note. set 1, then 0 is read
}
else if(bPort == SERIAL_PORT1)
{
UBRR1H = 0; UBRR1L = bBaudrate;
UCSR1A = 0x02; UCSR1B = 0x18;
if(bInterrupt&RX_INTERRUPT) sbi(UCSR1B,7); // RxD interrupt enable
UCSR1C = 0x06; UDR1 = 0xFF;
sbi(UCSR1A,6);//SET_TXD1_FINISH; // Note. set 1, then 0 is read
}
}

/*
TxD8Hex() print data seperatly.
ex> 0x1a -> ‘1’ ‘a’.
*/
void TxD8Hex(byte bSentData)
{
byte bTmp;

bTmp =((byte)(bSentData>>4)&0x0f) + (byte)‘0’;
if(bTmp > ‘9’) bTmp += 7;
TxD8(bTmp);
bTmp =(byte)(bSentData & 0x0f) + (byte)‘0’;
if(bTmp > ‘9’) bTmp += 7;
TxD8(bTmp);
}

/*
TxD80() send data to USART 0.
*/
void TxD80(byte bTxdData)
{
while(!TXD0_READY);
TXD0_DATA = bTxdData;
}

/*
TXD81() send data to USART 1.
*/
void TxD81(byte bTxdData)
{
while(!TXD1_READY);
TXD1_DATA = bTxdData;
}

/*
TXD32Dex() change data to decimal number system
*/
void TxD32Dec(long lLong)
{
byte bCount, bPrinted;
long lTmp,lDigit;
bPrinted = 0;
if(lLong < 0)
{
lLong = -lLong;
TxD8(’-’);
}
lDigit = 1000000000L;
for(bCount = 0; bCount < 9; bCount++)
{
lTmp = (byte)(lLong/lDigit);
if(lTmp)
{
TxD8(((byte)lTmp)+‘0’);
bPrinted = 1;
}
else if(bPrinted) TxD8(((byte)lTmp)+‘0’);
lLong -= ((long)lTmp)*lDigit;
lDigit = lDigit/10;
}
lTmp = (byte)(lLong/lDigit);
/if(lTmp)/ TxD8(((byte)lTmp)+‘0’);
}

/*
TxDString() prints data in ACSII code.
*/
void TxDString(byte *bData)
{
while(*bData)
{
TxD8(*bData++);
}
}

/*
RxD81() read data from UART1.
RxD81() return Read data.
*/
byte RxD81(void)
{
while(!RXD1_READY);
return(RXD1_DATA);
}

/*
SIGNAL() UART0 Rx Interrupt - write data to buffer
/
SIGNAL (SIG_UART0_RECV)
{
gbpRxInterruptBuffer(gbRxBufferWritePointer++)] = RXD0_DATA;
}[/code]*******************************************************

[innerbreed]

looking at the code i have found the part that gets the quad to lean or balance its cog while walking.

i beleive it to be this:

[code] double stride = 40; // 40;
double lean = 20; // 20;
double lift = 40; // 40

    limbs[0].mm[0] = +120 + lean * cos(t * pi / 2);Coxa?
    limbs[1].mm[0] = +120 + lean * cos(t * pi / 2);
    limbs[2].mm[0] = -120 + lean * cos(t * pi / 2);
    limbs[3].mm[0] = -120 + lean * cos(t * pi / 2);

    limbs[0].mm[1] = +120 + lean * sin(t * pi) + stride * step_u(t - 0.5);Femur?
    limbs[1].mm[1] = -120 + lean * sin(t * pi) + stride * step_u(t - 3.5);
    limbs[2].mm[1] = +120 + lean * sin(t * pi) + stride * step_u(t - 2.5);
    limbs[3].mm[1] = -120 + lean * sin(t * pi) + stride * step_u(t - 1.5);
    
    limbs[0].mm[2] = +80 - lift * step_v(t - 0.5);Tibia?
    limbs[1].mm[2] = +80 - lift * step_v(t - 3.5);
    limbs[2].mm[2] = +80 - lift * step_v(t - 2.5);
    limbs[3].mm[2] = +80 - lift * step_v(t - 1.5);  [/code]

ill be working on this and other parts of the code to get it to work with atom pro.

any insight or input.

[xan]

Hi Innerbreed,

V1.3 includes Zenta’s balance calculations. One of the things it does it calculate the middle position between the legs and places the body there. This is shown in one of his videos. What you need to do is take this calculations and exclude the flying leg. This should place the body between the other legs which are on the ground. You need to build something around it which places the body between the 3 legs before you lift the leg but it should do the job.

Xan

[innerbreed]

having watched this vid a few times… (about 10) im very impressed with the balance calculations. “love the vid Zenta”.

yes your right about taking the calculations and exclude the flying leg and this is the of code im trying to write.

once i have all my servos i can start compliling the code. well i could start now as i have everything but 7 servos.

One of the things it does it calculate the middle position between the legs and places the body there.

Im using V1.3 size=75[/size] so ill try and place these calculation once i figure them out.

cheers.

[likwidchz]

Yeah Innerbreed, that video is amazing… that’s why I had to buy that T7C transmitter I wanted that exact same thing he has… It’s just TOO COOL. and well worth it as a learning experience

I think more people should be using RC vs teatherd ps2 or wireless ps2… the RC is slick!

yes that video is quite amazing. good job Zenta.

[innerbreed]

@KM6VV from stalker project

Yes, I had been there briefly, but hadn’t made it back. These videos would be good to reference there as well.

Have you worked out any sort of “phase chart” for what you want to do with your body IK and leg IK? Might be interesting if something could be worked out in a spread sheet, that would generate a chart or graph.

Alan KM6VV

@KM6VV

That’s certainly a nice walking quadruped!

Are you saying that the body shifts around to move the COG as it walks? the gait looks the same as this one:

youtube.com/watch?v=lxIy3jYuQCo&feature=related

But I guess it does move it’s body around more. I like this video for explaining a quad gait; although I’m told it’s not very good?

youtube.com/watch?v=wt_k2aoF6l0

I can get a feel of moving the body 1/4 the travel as each leg moves in succession.

And now the notion of “Body IK” has been introduced. I guess I’m way behind!

Alan KM6VV

@innerbreed

Here you can see the idea i wish to impliment into my code.

youtube.com/watch?v=Q2YAZHRZ … re=related

i need to work out how to program this.
i think the intended plane would be Z & X and the body would have to move
into the triangle created by the tripod in each step. (as each leg lifts)

im not sure how to get these globe positions “on the fly” and add them to the code.
any help will be critical.

What you need to do is take this calculations and exclude the flying leg. This should place the body between the other legs which are on the ground. You need to build something around it which places the body between the 3 legs before you lift the leg but it should do the job.

Xan

with this being said i would also need to get the timing right so that once
the COG has been transfered away from the fly leg i will need it to return
back when leg is returning to ground again.

any ideas?