Introduction: Intellisaurus
Intellisaurus is intended for many skill levels. The easy to follow instructions allow someone with a modest skill level to assemble the robot and have it walking in a matter of hours. There is no soldering or programming required to build and play with it.
This fun loving guy reaches 8 inches tall. He sports an Arduino Nano to control the motors and a Raspberry Pi Zero W to connect to the internet and run Google Assistant, or other various AI algorithms.All open source software, so you are in control, modify, and improve your Intellisaurus.
Step 1: Materials (Off the Shelf)
- Hardware
- 10 - MG966s Servos https://www.amazon.com/gp/product/B07BVR816V
- Arduino Nano https://www.amazon.com/gp/product/B07G99NNXL
- Servo Controller board PCA9685 https://www.amazon.com/gp/product/B07G99NNXL
- Sharp Distance Sensor 10 cm ( GP2Y0D810Z0F) https://www.amazon.com/gp/product/B07G99NNXL
- Battery Holder for 2 18650 https://www.amazon.com/gp/product/B07G99NNXL
- 5 volt @ 3 Amp Voltage Regulator https://www.amazon.com/gp/product/B07G99NNXL
- 5 volt @ 3 Amp Voltage Regulator (for Servos) https://www.amazon.com/gp/product/B07G99NNXL
- On/Off Switch https://www.amazon.com/gp/product/B07G99NNXL
- IR Remote control with Sensor https://www.amazon.com/gp/product/B07G99NNXL
- MPU6050 Sensor https://www.amazon.com/gp/product/B07G99NNXL
- 3" Speaker https://www.amazon.com/gp/product/B07G99NNXL
- Nut & Bolt
- M2 x 25mm bolt https://www.amazon.com/gp/product/B07G99NNXL
- M2 x 12 mm bolt - 33 https://www.amazon.com/gp/product/B07G99NNXL
- M3 x 10mm bolt - 52 https://www.amazon.com/gp/product/B07G99NNXL
- M3 Nylon nuts - 61 https://www.amazon.com/gp/product/B07G99NNXL
- M2 nuts - 38 https://www.amazon.com/gp/product/B07G99NNXL
- M2 x 6mm Nylon hex standoffs - 10 https://www.amazon.com/gp/product/B07G99NNXL
- M3 x 50mm bolt - 6 https://www.amazon.com/gp/product/B07G99NNXL
Step 2: Material ( 3D Printed/Acryclic)
- 3D Printed Parts
- Simply print one of each stl file
- Note Use Frame Version
- Acryclic Part
- 1x Neck Holder 1 & 2
- 2x Everything else
STL File(Shell) : https://github.com/JacquinBuchanan/Intellisaurus/t...
STL File (Leg) : https://github.com/JacquinBuchanan/Intellisaurus/t...
SVG File (Frame) : https://github.com/JacquinBuchanan/Intellisaurus/t...
Step 3: Assembly : Frame
Step 4: Assembly : Frame Continue
Step 5: Assembly : Internal Wiring
Step 6: Assembly : Leg
Step 7: Assembly : Shell
Step 8: Code
Paste following code into your arduino IDE
<p>/***************************************************<br> Quadropewd Walking Dinosour Robot</p><p> copyright Jacquin Buchanan 2018
****************************************************/
#include "DinoRemoteControl.h"
#include <MPU6050_tockn.h"
#include <Wire.h>
#include <EEPROM.h>
#include <Adafruit_PWMServoDriver.h>
</p><p>// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
MPU6050 mpu6050(Wire);</p><p>// Define IR distance sensor model and input pin:
#define IRPin 3</p><p>// Choose the IR protocol of your remote. this isi the one shipped in the kit
CNec IRLremote;</p><p>#define MIN_PULSE_WIDTH 450
#define MAX_PULSE_WIDTH 2350
#define FREQUENCY 50</p><p>#define BACK_LEG_L1 86
#define BACK_LEG_L2 68
#define BACK_LEG_HIP_OFFSET_ANGLE 65.1
#define BACK_LEG_KNEE_OFFSET_ANGLE (-57.2)</p><p>#define FRONT_LEG_L1 60
#define FRONT_LEG_L2 48
#define FRONT_LEG_HIP_OFFSET_ANGLE 62
#define FRONT_LEG_KNEE_OFFSET_ANGLE (-64.1)</p><p>#define BACK_TO_FRONT_HIP_DIF 43.6</p><p>//These are the amount each motor gear is off from true because of the 7 degree gear size;
// these are stored in the EPROM
float MotorBuildOffsets[10] = {0.0, 0.0 , 0.0 , 0.0 , 0.0 , 0.0, 0.0, 0.0, 0.0, 0.0};
int CurrentMotorAdjusting = -1;</p><p>// The distance off the ground to hip of a normal walk
#define NORMAL_WALK_HEIGHT 98</p><p>// The distance off the ground to raise foot of a normal walk
float FootRaise[5] = {20.0, 20.0, 15.0, 10.0, 10.0};</p><p>#define MIN_FOOT_HEIGHT 70
#define MAX_FOOT_HEIGHT 100</p><p>#define MIN_FOOT_X -40
#define MAX_FOOT_X 40
#define NORMAL_STEP_LENGTH 55</p><p>#define FRONT_STEP_OFFSET_FROM_HIP_FORWARDS 0
#define BACK_STEP_OFFSET_FROM_HIP_FORWARDS 10</p><p>#define FRONT_STEP_OFFSET_FROM_HIP_BACKWARDS 15
#define BACK_STEP_OFFSET_FROM_HIP_BACKWARDS 15</p><p>int speedIndex = 0;
float StepTime[5] = {1000, 700, 500, 400, 400}; //the time for one foot step forward. (i.e. 1/4 the entire walk cycle)
float RightSideStepLength = NORMAL_STEP_LENGTH; // mm distance that each foot moves forward. NOTE this is differnt for left and right to allow turning.
float LeftSideStepLength = NORMAL_STEP_LENGTH; // mm distance that each foot moves forward. NOTE this is differnt for left and right to allow turning.</p><p>boolean leftBackwards = false;
boolean rightBackwards = false;</p><p>//four phases to a walk. One for each foot moving forward
// 0 is back right,
// 1 is front right
// 2 is back left
// 3 is front left
int walkphase = 0;</p><p>int walkPhaseIndex = 0;</p><p>int backwardsWalkPhases[4] = {1,0,3,2};</p><p>//The start time for each foots phase
unsigned long phaseStartTime[4];
float PhaseStartFootXPos[4];
float FootXPos[4] = {0, 0, 0, 0};
float FootYPos[4] = {NORMAL_WALK_HEIGHT, NORMAL_WALK_HEIGHT, NORMAL_WALK_HEIGHT, NORMAL_WALK_HEIGHT};
boolean FootStanding[4];</p><p>//mode is used to decide what the motors are doing
// 0 is tweaking motor offsets
// 1 is standing still
// 2 is slow walking
// 3 is starting to walk, (i.e. shift weight)
// 4 is jogging
uint8_t mode;</p><p>//PID balancing variables
double last_error_LR = 0.0;
double Kp_LR = 0.02;//0.05;
double Kd_LR = 0.17;//0.17;
double K_PLR = 1.2;
double integrated_error_LR = 0.0;
double Ki_LR = 0.0001;</p><p>double last_error_FB = 0.0;
double Kp_FB = 0.02;//0.8;
double Kd_FB = 0.17;//0.27;
double K_PFB = 1.2;
double integrated_error_FB = 0.0;
double Ki_FB = 0.0001;</p><p>double GUARD_GAIN = 20.0;</p><p>float xBalanceOffset = 0.0;
float yBalanceOffset = 0.0;</p><p>#define FIRST_STEP_LEAN_ANGLE 12
#define FIRST_STEP_TAIL_ANGLE 15
#define FIRST_STEP_HEAD_ANGLE 15</p><p>float LeftRightShiftAngle[5] = {7.0, 7.0, 4.0, 3.0, 0.0};
float TailShiftAngle[5] = {15.0, 15.0, 5.0, 3.0, 0.0};
float HeadShiftAngle[5] = {7.0, 5.0, 5.0, 3.0, 0.0};</p><p>float angle0 = 90;
float angle1 = 90;
float angle2 = 90;
float angle3 = 90;
float angle4 = 90;
float angle5 = 90;
float angle6 = 90;
float angle7 = 90;</p><p>#define TAIL_WIRE 8
#define NECK_WIRE 9
#define HEAD_WIRE 10</p><p>float TailPosition = 0.0;
float NeckPosition = 0.0;
float HeadPosition = 0.0;</p><p>float TailAngle = 90;
float NeckAngle = 90;
float HeadAngle = 90;</p><p>#define TAIL_POSITION_MAX (+45)
#define TAIL_POSITION_MIN (-45)</p><p>#define NECK_POSITION_MAX (+15)
#define NECK_POSITION_MIN (-15)</p><p>#define HEAD_POSITION_MAX (+12)
#define HEAD_POSITION_MIN (-22)</p><p>char MSG_STOP_AND_STAND[] = "*** Stop and stand ";</p><p>/***************************************************
Main Setup function</p><p> *******************************************/
void setup() {</p><p> Wire.begin();</p><p> Serial.begin(9600);
Serial.println("Walking Dinosour Robot!");</p><p> // Servo controller setup
pwm.begin();
pwm.setPWMFreq(FREQUENCY);</p><p> // Start reading the remote. PinInterrupt or PinChangeInterrupt* will automatically be selected
if (!IRLremote.begin(IR_REMOTE_RECIVER_PIN))
Serial.println(F("You did not choose an infrared remote input pin."));</p><p> pinMode(IRPin, INPUT);</p><p> //read the motor offsets from the EPROM
if (EEPROM.read(0) == 127)
{
for (int i = 0; i < 10; i++) {
MotorBuildOffsets[i] = ((float)EEPROM.read(i + 1) - 128) / 2;
//Serial.println(MotorBuildOffsets[i]);
}
}</p><p> mpu6050.begin();
float gyroXoffset = 0.0;
float gyroYoffset = 0.0;</p><p> // the gyrooffset are stored at a specific location above the motor offsets
if (EEPROM.read(20) == 127)
{
gyroXoffset = ((float)EEPROM.read(21) - 128) / 2;
gyroYoffset = ((float)EEPROM.read(22) - 128) / 2;
}</p><p> // I assume these offset are based on the mounting of the devcie in the robot
mpu6050.setGyroOffsets(gyroXoffset, gyroYoffset, 0.0);</p><p> //Start out just standing
mode = 1;
CurrentMotorAdjusting = -1;
StandPose();</p><p>}</p><p>/***************************************************
Main Loop function</p><p> *******************************************/
void loop() {</p><p> mpu6050.update();
// Look for some user control
while ( RemoteControlCodeAvailable())
{</p><p> int inByte = RemoteControlCodeRead();
switch (inByte)
{</p><p> case BUTTON_0 :
if( mode != 0) {</p><p> //start jogging
//Serial.println("*** Start jogging forward ");
leftBackwards = false;
rightBackwards = false;
mode = 4;
unsigned long NowTime = millis();
walkPhaseIndex = 0;
walkphase = 0;
speedIndex = 4;
RightSideStepLength = NORMAL_STEP_LENGTH;
LeftSideStepLength = NORMAL_STEP_LENGTH;</p><p> for (int FootID = 0; FootID < 4; FootID++)
{
phaseStartTime[FootID] = NowTime;
}
PhaseStartFootXPos[0] = FootXPos[0];
PhaseStartFootXPos[1] = FootXPos[1];
PhaseStartFootXPos[2] = FootXPos[2];
PhaseStartFootXPos[3] = FootXPos[3];
break;
}
case BUTTON_1 :
if (mode == 0)
{
CurrentMotorAdjusting = inByte - BUTTON_0;</p><p> } else {
TailPosition += 10;
MoveTail();
}
break;
case BUTTON_2 :</p><p> if (mode == 0)
{
CurrentMotorAdjusting = inByte - BUTTON_0;</p><p> } else {
HeadPosition += 10;
MoveHead();
}
break;
case BUTTON_3 :
if (mode == 0)
{
CurrentMotorAdjusting = inByte - BUTTON_0;</p><p> }
break;
case BUTTON_4 :</p><p> if (mode == 0)
{
CurrentMotorAdjusting = inByte - BUTTON_0;</p><p> } else {</p><p>
NeckPosition += 10;
MoveNeck();
}
break;
case BUTTON_5 :</p><p> if (mode == 0)
{
CurrentMotorAdjusting = inByte - BUTTON_0;</p><p> } else {
TailPosition = 0.0;
NeckPosition = 0.0;
HeadPosition = 0.0;
MoveTail();
MoveNeck();
MoveHead();
}
break;
case BUTTON_6 :
if (mode == 0)
{
CurrentMotorAdjusting = inByte - BUTTON_0;</p><p> } else {
NeckPosition -= 10;
MoveNeck();
}
break;
case BUTTON_7 :
if (mode == 0)
{
CurrentMotorAdjusting = inByte - BUTTON_0;</p><p> } else {
TailPosition -= 10;
MoveTail();
}
break;</p><p> case BUTTON_8 :
if (mode == 0)
{
CurrentMotorAdjusting = inByte - BUTTON_0;</p><p> } else {
HeadPosition -= 10;
MoveHead();
}
break;</p><p>
case BUTTON_9 :
if (mode == 0)
{
CurrentMotorAdjusting = inByte - BUTTON_0;</p><p> }
break;</p><p> case BUTTON_UP:
if (mode == 0)
{
if (CurrentMotorAdjusting >= 0) {
MotorBuildOffsets[CurrentMotorAdjusting] += 0.5;</p><p> EEPROM.write(0, 127);
for (int i = 0; i < 10; i++)
{
EEPROM.write(i + 1, (byte)((int)(MotorBuildOffsets[i] * 2) + 128));
}</p><p> Serial.print("CurrentMotorAdjusting ");
Serial.print(CurrentMotorAdjusting);
Serial.print("\t v : ");
Serial.println(MotorBuildOffsets[CurrentMotorAdjusting]);</p><p> //Recalculate the pose to move the motor
StandPose();
}
} else {</p><p> if (mode == 2 || mode == 4) {</p><p> if(leftBackwards && rightBackwards)
{</p><p> if (mode == 4)
{
// come out of jog and into walk
speedIndex = 3;
mode = 2;
} else {
// We are walking backwards so slow down
// Slow down
speedIndex -= 1;
if (speedIndex < 0)
{
speedIndex = 0;
// Stop and stand
mode = 1;
StandPose();
Serial.println(MSG_STOP_AND_STAND);
}
}
Serial.print("*** Walk backwards slower "); Serial.println(speedIndex);
} else {
// We are already walking forwards so walk faster
speedIndex += 1;
if (speedIndex >= 5)
{
speedIndex = 4;
}</p><p> if(speedIndex == 4)
{
//speed index 4 is a jog
mode = 4;
}
Serial.print("*** Walk forwards faster "); Serial.println(speedIndex);</p><p> }
} else {
// Start walking forward</p><p> Serial.println("*** Start walking forward ");
leftBackwards = false;
rightBackwards = false;
mode = 3;
unsigned long NowTime = millis();
walkPhaseIndex = 0;
speedIndex = 0;
phaseStartTime[0] = NowTime;</p><p> }</p><p> RightSideStepLength = NORMAL_STEP_LENGTH;
LeftSideStepLength = NORMAL_STEP_LENGTH;</p><p> }
break;</p><p> case BUTTON_DOWN:</p><p> if (mode == 0)
{
if (CurrentMotorAdjusting >= 0) {
MotorBuildOffsets[CurrentMotorAdjusting] -= 0.5;</p><p> EEPROM.write(0, 127);
for (int i = 0; i < 10; i++)
EEPROM.write(i + 1, (byte)((int)(MotorBuildOffsets[i] * 2) + 128));</p><p> Serial.print("CurrentMotorAdjusting ");
Serial.print(CurrentMotorAdjusting);
Serial.print("\t v : ");
Serial.println(MotorBuildOffsets[CurrentMotorAdjusting]);</p><p> //Recalculate the pose to move the motor
StandPose();
}
} else {</p><p> if (mode == 2 || mode == 4) {</p><p> if(leftBackwards && rightBackwards)
{
// We are already walking backwards so speed up
speedIndex += 1;
if (speedIndex >= 5)
{
speedIndex = 4;
}</p><p> if(speedIndex == 4)
{
//speed index 4 is a jog
mode = 4;
}
Serial.print("*** Walk backwards faster "); Serial.println(speedIndex);
} else {
if (mode == 4)
{
// come out of jog and into walk
speedIndex = 3;
mode = 2;
} else {</p><p> // We are walking forwards so Slow down
speedIndex -= 1;
if (speedIndex < 0) {
speedIndex = 0;
// Stop and stand
mode = 1;
StandPose();
Serial.println(MSG_STOP_AND_STAND);
}
}
Serial.print("*** Walk forwads slower "); Serial.println(speedIndex);
}
} else {
// Start walking backwards
Serial.println("*** Start walking backwards ");
mode = 3;
unsigned long NowTime = millis();
walkPhaseIndex = 0;
speedIndex = 0;
phaseStartTime[0] = NowTime;
leftBackwards = true;
rightBackwards = true;
}</p><p> RightSideStepLength = NORMAL_STEP_LENGTH;
LeftSideStepLength = NORMAL_STEP_LENGTH;</p><p> }
break;</p><p> case BUTTON_RIGHT:
// Turn Right
Serial.println("*** Turn right ");</p><p> if (mode == 2 || mode == 3 || mode == 4) {
if(LeftSideStepLength < NORMAL_STEP_LENGTH)
{
LeftSideStepLength += 10;
LeftSideStepLength = constrain(LeftSideStepLength, 20.0 , NORMAL_STEP_LENGTH);
RightSideStepLength = NORMAL_STEP_LENGTH;
}else {
RightSideStepLength -= 10;
RightSideStepLength = constrain(RightSideStepLength, 20.0 , NORMAL_STEP_LENGTH);
LeftSideStepLength = NORMAL_STEP_LENGTH;
}
} else {
CurrentMotorAdjusting = -1;</p><p> leftBackwards = false;
rightBackwards = true;</p><p> //very short steps for turning in place
RightSideStepLength = 2*NORMAL_STEP_LENGTH/3;
LeftSideStepLength = NORMAL_STEP_LENGTH;</p><p> speedIndex = 3;
mode = 3;
walkPhaseIndex = 0;
unsigned long NowTime = millis();
for (int FootID = 0; FootID < 4; FootID++)
{
//This is wrong for intial step but fix it later
phaseStartTime[FootID] = NowTime - (FootID * (StepTime[speedIndex] / 3));
}
}
break;</p><p> case BUTTON_LEFT:
// Turn left
Serial.println("*** Turn left ");</p><p> if (mode == 2|| mode == 3 || mode == 4) {
if(RightSideStepLength < NORMAL_STEP_LENGTH)
{
RightSideStepLength += 10;
RightSideStepLength = constrain(RightSideStepLength, 20.0 , NORMAL_STEP_LENGTH);
LeftSideStepLength = NORMAL_STEP_LENGTH;
} else {
LeftSideStepLength -= 10;
LeftSideStepLength = constrain(LeftSideStepLength, 20.0 , NORMAL_STEP_LENGTH);
RightSideStepLength = NORMAL_STEP_LENGTH;
}
} else {</p><p> CurrentMotorAdjusting = -1;
leftBackwards = true;
rightBackwards = false;
//very short steps for turning in place
RightSideStepLength = NORMAL_STEP_LENGTH;
LeftSideStepLength = 2*NORMAL_STEP_LENGTH/3;</p><p> speedIndex = 3;</p><p> mode = 3;
walkPhaseIndex = 0;
unsigned long NowTime = millis();
for (int FootID = 0; FootID < 4; FootID++)
{
//This is wrong for intial step but fix it later
phaseStartTime[FootID] = NowTime - (FootID * (StepTime[speedIndex] / 3));
}
}
break;</p><p> case BUTTON_STAR:</p><p> if(mode == 0)
{
Serial.println("*** Calibrating GPU. Make sure robot is standing on flat ground. ");
mpu6050.calcGyroOffsets(true);
EEPROM.write(20, 127);
EEPROM.write(21, (byte)((int)(mpu6050.getGyroXoffset() * 2) + 128));
EEPROM.write(22, (byte)((int)(mpu6050.getGyroYoffset() * 2) + 128));
} else {
Serial.println("*** Go Straight ");
if (mode == 2|| mode == 3 || mode == 4) {
LeftSideStepLength = NORMAL_STEP_LENGTH;
RightSideStepLength = NORMAL_STEP_LENGTH;
}
}</p><p> break;</p><p> case BUTTON_CENTER :
// Stop and stand
mode = 1;
CurrentMotorAdjusting = -1;</p><p> StandPose();</p><p> Serial.println(MSG_STOP_AND_STAND);</p><p> break;</p><p> case BUTTON_HASH :
// Motor setup
mode = 0;
CurrentMotorAdjusting = -1;</p><p> StandPose();
Serial.println("*** Motor Adjusting Offset Mode");
break;</p><p> }
}</p><p> if (mode == 1)
{</p><p> xBalanceOffset = 0.0;
// Use the forward IR sensor to see if we are on ground
int NoGround = digitalRead(IRPin);</p><p> if(NoGround == 1)
{
//We arn't on the ground so just stand straight
StandPose();
} else {
LeftRightBalance(-1, 0.0);
FrontBackBalance(-1, -1);
}</p><p> FootYPos[0] = constrain(FootYPos[0], MIN_FOOT_HEIGHT , MAX_FOOT_HEIGHT);
FootYPos[1] = constrain(FootYPos[1], MIN_FOOT_HEIGHT , MAX_FOOT_HEIGHT);
FootYPos[2] = constrain(FootYPos[2], MIN_FOOT_HEIGHT , MAX_FOOT_HEIGHT);
FootYPos[3] = constrain(FootYPos[3], MIN_FOOT_HEIGHT , MAX_FOOT_HEIGHT);</p><p>
FootXPos[0] = constrain(FootXPos[0], MIN_FOOT_X , MAX_FOOT_X);
FootXPos[1] = constrain(FootXPos[1], MIN_FOOT_X , MAX_FOOT_X);
FootXPos[2] = constrain(FootXPos[2], MIN_FOOT_X , MAX_FOOT_X);
FootXPos[3] = constrain(FootXPos[3], MIN_FOOT_X , MAX_FOOT_X);</p><p> MoveLegTo(FootXPos[0], FootYPos[0] + BACK_TO_FRONT_HIP_DIF, 0);
MoveLegTo(FootXPos[2], FootYPos[2] + BACK_TO_FRONT_HIP_DIF, 2);
MoveLegTo(FootXPos[1], FootYPos[1], 1);
MoveLegTo(FootXPos[3], FootYPos[3], 3);</p><p> }</p><p> if (mode == 3)
{
//shift weight to start walking
unsigned long NowTime = millis();</p><p> if(NowTime > phaseStartTime[0] + StepTime[speedIndex]/2) {</p><p> //Really Start walking
mode = 2;
walkPhaseIndex = 0;
walkphase = 0;
if(rightBackwards && leftBackwards)
walkphase = backwardsWalkPhases[walkPhaseIndex];
for (int FootID = 0; FootID < 4; FootID++)
{
//This is wrong for intial step but fix it later
if(rightBackwards && leftBackwards)
phaseStartTime[backwardsWalkPhases[FootID]] = NowTime - (FootID * (StepTime[speedIndex] / 3));
else
phaseStartTime[FootID] = NowTime - (FootID * (StepTime[speedIndex] / 3));</p><p> FootStanding[FootID] = true;
}
PhaseStartFootXPos[1] = FootXPos[1] = 0;
PhaseStartFootXPos[3] = FootXPos[3] = 0;</p><p> if(rightBackwards || leftBackwards)
{
//If we are walking backwards then slide the back feet forward a little to get started
FootXPos[0] = BACK_STEP_OFFSET_FROM_HIP_BACKWARDS*2;
FootXPos[1] = FRONT_STEP_OFFSET_FROM_HIP_BACKWARDS;
FootXPos[2] = BACK_STEP_OFFSET_FROM_HIP_BACKWARDS*2;
FootXPos[3] = FRONT_STEP_OFFSET_FROM_HIP_BACKWARDS;
}</p><p>
} else {</p><p> float WeightshiftPercentage = (float)(NowTime - (phaseStartTime[0])) / (StepTime[speedIndex]/2);
WeightshiftPercentage = constrain(WeightshiftPercentage, 0.0 , 1.0);</p><p> xBalanceOffset = GetRelativeValue( 0, FIRST_STEP_LEAN_ANGLE, WeightshiftPercentage);</p><p> TailPosition = GetRelativeValue( 0.0, FIRST_STEP_TAIL_ANGLE, WeightshiftPercentage);
NeckPosition = GetRelativeValue( 0.0, FIRST_STEP_HEAD_ANGLE, WeightshiftPercentage);
MoveTail();
MoveNeck();</p><p> if(rightBackwards || leftBackwards)
{
//If we are walking backwards then slide the back feet forward a little to get started
FootXPos[0] = GetRelativeValue( 0, BACK_STEP_OFFSET_FROM_HIP_BACKWARDS*2, WeightshiftPercentage); //RightSideStepLength / 2 + BACK_STEP_OFFSET_FROM_HIP_BACKWARDS, WeightshiftPercentage);
FootXPos[1] = GetRelativeValue( 1, FRONT_STEP_OFFSET_FROM_HIP_BACKWARDS, WeightshiftPercentage);
FootXPos[2] = GetRelativeValue( 2, BACK_STEP_OFFSET_FROM_HIP_BACKWARDS*2, WeightshiftPercentage); // LeftSideStepLength / 2 + BACK_STEP_OFFSET_FROM_HIP_BACKWARDS, WeightshiftPercentage);
FootXPos[3] = GetRelativeValue( 3, FRONT_STEP_OFFSET_FROM_HIP_BACKWARDS, WeightshiftPercentage);
}</p><p> MoveLegsForWalk( 1.0);
}</p><p> //Serial.print(xBalanceOffset);Serial.print(" ***PHASE - ");Serial.println(walkphase);
}</p><p> // Use the forward IR sensor to see if we are about to walk off a cliff.
int NoGround = digitalRead(IRPin);
if (mode == 2 || mode == 4)
{
//this is a precheck for mode 2 walking forward
if(!(rightBackwards && leftBackwards))
{</p><p> if( NoGround == 1)
{
// Stop and stand
mode = 1;
CurrentMotorAdjusting = -1;
StandPose();
Serial.println("*** Stop and stand *** Cliff or no ground ");
}
}
}
if (mode == 2)
{
//This is the true walking mode
// One leg off the ground at a time.
boolean HighStep = true;
unsigned long NowTime = millis();</p><p> unsigned long WalkPhaseExtraTime = StepTime[speedIndex]/4;</p><p> float LeftRightOffsetAngle = LeftRightShiftAngle[speedIndex];
float TailOffsetAngle = TailShiftAngle[speedIndex];
float HeadOffsetAngle = HeadShiftAngle[speedIndex];
if(FootStanding[0] || FootStanding[1] || FootStanding[2] || FootStanding[3] )//||HighStep)
{</p><p> LeftRightOffsetAngle = FIRST_STEP_LEAN_ANGLE;
TailOffsetAngle = FIRST_STEP_TAIL_ANGLE;
HeadOffsetAngle = FIRST_STEP_HEAD_ANGLE;
}
if (NowTime > phaseStartTime[walkphase] + StepTime[speedIndex]) {</p><p> phaseStartTime[walkphase] = NowTime;
PhaseStartFootXPos[walkphase] = FootXPos[walkphase];</p><p> walkPhaseIndex++;
if (walkPhaseIndex > 3) walkPhaseIndex = 0;</p><p> walkphase = walkPhaseIndex;</p><p> if(rightBackwards && leftBackwards)
walkphase = backwardsWalkPhases[walkPhaseIndex];</p><p> phaseStartTime[walkphase] = NowTime;
PhaseStartFootXPos[walkphase] = FootXPos[walkphase];</p><p> //Serial.print(" ***PHASE - ");Serial.println(walkphase);</p><p> }</p><p> float StepPercentage = (float)(NowTime - phaseStartTime[walkphase]) / (StepTime[speedIndex] - WalkPhaseExtraTime);
if(StepPercentage > 1.0) StepPercentage = 1.0;
float footHeight = FootRaise[speedIndex] * (-1 * pow((StepPercentage*2) - 1, 2) +1);
//FootRaise[speedIndex] * (1 - pow(40 , StepPercentage) / 40);
FootYPos[walkphase] = NORMAL_WALK_HEIGHT - footHeight;</p><p> float WeightshiftPercentage = 0.0;</p><p> //if (NowTime > phaseStartTime[walkphase] + WalkPhaseExtraTime)
WeightshiftPercentage = (float)(NowTime - (phaseStartTime[walkphase])) / StepTime[speedIndex];</p><p> WeightshiftPercentage = constrain(WeightshiftPercentage, 0.0 , 1.0);</p><p> float weightshift = (pow(40 , WeightshiftPercentage) / 40);</p><p> float DirectionalRightSideStepLength = RightSideStepLength;
if(rightBackwards)
DirectionalRightSideStepLength = -RightSideStepLength;</p><p> float DirectionalLeftSideStepLength = LeftSideStepLength;
if(leftBackwards)
DirectionalLeftSideStepLength = -LeftSideStepLength;</p><p> switch (walkphase) {</p><p> case 0:</p><p> if(rightBackwards)
{
xBalanceOffset = GetRelativeValue( LeftRightOffsetAngle, -LeftRightOffsetAngle, weightshift);
TailPosition = GetRelativeValue(TailOffsetAngle, -TailOffsetAngle, weightshift);
NeckPosition = GetRelativeValue(HeadOffsetAngle, -HeadOffsetAngle, weightshift);
MoveTail();
MoveNeck();
}
else
xBalanceOffset = LeftRightOffsetAngle;</p><p> // 0 is back right is lifted
//calculate x as a function of time
SetFootXPosition( 0, PhaseStartFootXPos[0], DirectionalRightSideStepLength / 2 , WalkPhaseExtraTime);
SetFootXPosition( 1, PhaseStartFootXPos[1], DirectionalRightSideStepLength / 2 , WalkPhaseExtraTime);
SetFootXPosition( 2, PhaseStartFootXPos[2], DirectionalLeftSideStepLength / 2 , WalkPhaseExtraTime);
SetFootXPosition( 3, PhaseStartFootXPos[3], DirectionalLeftSideStepLength / 2 , WalkPhaseExtraTime);</p><p> break;</p><p> case 1:</p><p> if(rightBackwards)
xBalanceOffset = LeftRightOffsetAngle;
else {
xBalanceOffset = GetRelativeValue( LeftRightOffsetAngle, -LeftRightOffsetAngle, weightshift);</p><p> TailPosition = GetRelativeValue(TailOffsetAngle, -TailOffsetAngle, weightshift);
NeckPosition = GetRelativeValue(HeadOffsetAngle, -HeadOffsetAngle, weightshift);
MoveTail();
MoveNeck();
}</p><p> // 1 is front right is lifted
//calculate x as a function of time
SetFootXPosition( 0, PhaseStartFootXPos[0], DirectionalRightSideStepLength / 2 , WalkPhaseExtraTime);
SetFootXPosition( 1, PhaseStartFootXPos[1], DirectionalRightSideStepLength / 2 , WalkPhaseExtraTime);
SetFootXPosition( 2, PhaseStartFootXPos[2], DirectionalLeftSideStepLength / 2 , WalkPhaseExtraTime);
SetFootXPosition( 3, PhaseStartFootXPos[3], DirectionalLeftSideStepLength / 2 , WalkPhaseExtraTime);</p><p> break;</p><p> case 2:
// 2 is back left is lifted
if(leftBackwards)
{
xBalanceOffset = GetRelativeValue( -LeftRightOffsetAngle, LeftRightOffsetAngle, weightshift);
TailPosition = GetRelativeValue(-TailOffsetAngle, TailOffsetAngle, weightshift);
NeckPosition = GetRelativeValue(-HeadOffsetAngle, HeadOffsetAngle, weightshift);
MoveTail();
MoveNeck();
} else
xBalanceOffset = -LeftRightOffsetAngle;</p><p> //calculate x as a function of time
SetFootXPosition( 0, PhaseStartFootXPos[0], DirectionalRightSideStepLength / 2 , WalkPhaseExtraTime);
SetFootXPosition( 1, PhaseStartFootXPos[1], DirectionalRightSideStepLength / 2 , WalkPhaseExtraTime);
SetFootXPosition( 2, PhaseStartFootXPos[2], DirectionalLeftSideStepLength / 2 , WalkPhaseExtraTime);
SetFootXPosition( 3, PhaseStartFootXPos[3], DirectionalLeftSideStepLength / 2 , WalkPhaseExtraTime);</p><p> break;</p><p> case 3:
// 3 is front left is lifted</p><p> if(leftBackwards)
xBalanceOffset = -LeftRightOffsetAngle;
else {
xBalanceOffset = GetRelativeValue( -LeftRightOffsetAngle, LeftRightOffsetAngle, weightshift);
TailPosition = GetRelativeValue(-TailOffsetAngle, TailOffsetAngle, weightshift);
NeckPosition = GetRelativeValue(-HeadOffsetAngle, HeadOffsetAngle, weightshift);
MoveTail();
MoveNeck();
}
</p><p> //calculate x as a function of time
SetFootXPosition( 0, PhaseStartFootXPos[0], DirectionalRightSideStepLength / 2, WalkPhaseExtraTime);
SetFootXPosition( 1, PhaseStartFootXPos[1], DirectionalRightSideStepLength / 2, WalkPhaseExtraTime);
SetFootXPosition( 2, PhaseStartFootXPos[2], DirectionalLeftSideStepLength / 2, WalkPhaseExtraTime);
SetFootXPosition( 3, PhaseStartFootXPos[3], DirectionalLeftSideStepLength / 2, WalkPhaseExtraTime);</p><p> break;</p><p> }</p><p> MoveLegsForWalk( StepPercentage);</p><p> }</p><p> if (mode == 4)
{
// This is jogging mode, move two legs at a time</p><p> unsigned long NowTime = millis();</p><p> float LeftRightOffsetAngle = LeftRightShiftAngle[speedIndex];
if (NowTime > phaseStartTime[walkphase] + StepTime[speedIndex])
{</p><p> if (walkphase == 0)
walkphase = 1;
else
walkphase = 0;</p><p> for (int FootID = 0; FootID < 4; FootID++)
{
phaseStartTime[FootID] = NowTime;
PhaseStartFootXPos[FootID] = FootXPos[FootID];
}</p><p> }</p><p> float StepPercentage = (float)(NowTime - phaseStartTime[walkphase]) / StepTime[speedIndex] ;
if(StepPercentage > 1.0) StepPercentage = 1.0;
float footHeight = FootRaise[speedIndex] * (-1 * pow((StepPercentage*2) - 1, 2) +1);
//NOTE: there is no weight shift when jogging on four legs
xBalanceOffset = 0.0;
TailPosition = 0.0;
MoveTail();</p><p> float DirectionalRightSideStepLength = RightSideStepLength;
if(rightBackwards)
DirectionalRightSideStepLength = -RightSideStepLength;</p><p> float DirectionalLeftSideStepLength = LeftSideStepLength;
if(leftBackwards)
DirectionalLeftSideStepLength = -LeftSideStepLength;</p><p> float FrontDirectionalHipOffset = FRONT_STEP_OFFSET_FROM_HIP_FORWARDS;
float BackDirectionalHipOffset = BACK_STEP_OFFSET_FROM_HIP_FORWARDS;
if(rightBackwards || leftBackwards){
FrontDirectionalHipOffset = FRONT_STEP_OFFSET_FROM_HIP_BACKWARDS;
BackDirectionalHipOffset = BACK_STEP_OFFSET_FROM_HIP_BACKWARDS;
}</p><p> switch (walkphase) {</p><p> case 0:</p><p> // 0 is back right and front left are lifted
//calculate x as a function of time
FootXPos[0] = GetRelativeValue( PhaseStartFootXPos[0], DirectionalRightSideStepLength / 2 + BackDirectionalHipOffset, StepPercentage);
FootXPos[1] = GetRelativeValue( PhaseStartFootXPos[1], - DirectionalRightSideStepLength / 2 + FrontDirectionalHipOffset, StepPercentage);
FootXPos[2] = GetRelativeValue( PhaseStartFootXPos[2], - DirectionalLeftSideStepLength / 2 + BackDirectionalHipOffset, StepPercentage);
FootXPos[3] = GetRelativeValue( PhaseStartFootXPos[3], DirectionalLeftSideStepLength / 2 + FrontDirectionalHipOffset, StepPercentage);</p><p> FootYPos[0] = NORMAL_WALK_HEIGHT - footHeight;
FootYPos[1] = NORMAL_WALK_HEIGHT;
FootYPos[2] = NORMAL_WALK_HEIGHT;
FootYPos[3] = NORMAL_WALK_HEIGHT - footHeight;</p><p> //FrontBackBalance(0,3);</p><p> break;</p><p> case 1:</p><p> // 1 is front right and back left are lifted
//calculate x as a function of time
FootXPos[0] = GetRelativeValue( PhaseStartFootXPos[0], - DirectionalRightSideStepLength / 2 + BackDirectionalHipOffset, StepPercentage);
FootXPos[1] = GetRelativeValue( PhaseStartFootXPos[1], DirectionalRightSideStepLength / 2 + FrontDirectionalHipOffset, StepPercentage);
FootXPos[2] = GetRelativeValue( PhaseStartFootXPos[2], DirectionalLeftSideStepLength / 2 + BackDirectionalHipOffset, StepPercentage);
FootXPos[3] = GetRelativeValue( PhaseStartFootXPos[3], - DirectionalLeftSideStepLength / 2 + FrontDirectionalHipOffset, StepPercentage);</p><p> FootYPos[0] = NORMAL_WALK_HEIGHT;
FootYPos[1] = NORMAL_WALK_HEIGHT - footHeight;
FootYPos[2] = NORMAL_WALK_HEIGHT - footHeight;
FootYPos[3] = NORMAL_WALK_HEIGHT;
//FrontBackBalance(1,2);</p><p> break;
}
</p><p> FootXPos[0] = constrain(FootXPos[0], MIN_FOOT_X , MAX_FOOT_X);
FootXPos[1] = constrain(FootXPos[1], MIN_FOOT_X , MAX_FOOT_X);
FootXPos[2] = constrain(FootXPos[2], MIN_FOOT_X , MAX_FOOT_X);
FootXPos[3] = constrain(FootXPos[3], MIN_FOOT_X , MAX_FOOT_X);</p><p> FootYPos[0] = constrain(FootYPos[0], MIN_FOOT_HEIGHT , MAX_FOOT_HEIGHT);
FootYPos[1] = constrain(FootYPos[1], MIN_FOOT_HEIGHT , MAX_FOOT_HEIGHT);
FootYPos[2] = constrain(FootYPos[2], MIN_FOOT_HEIGHT , MAX_FOOT_HEIGHT);
FootYPos[3] = constrain(FootYPos[3], MIN_FOOT_HEIGHT , MAX_FOOT_HEIGHT);</p><p> MoveLegTo(FootXPos[0], FootYPos[0] + BACK_TO_FRONT_HIP_DIF, 0);
MoveLegTo(FootXPos[2], FootYPos[2] + BACK_TO_FRONT_HIP_DIF, 2);
MoveLegTo(FootXPos[1], FootYPos[1], 1);
MoveLegTo(FootXPos[3], FootYPos[3], 3);</p><p> }</p><p> SetServoAngle(0, angle0);
SetServoAngle(1, angle1);
SetServoAngle(2, angle2);
SetServoAngle(3, angle3);
SetServoAngle(4, angle4);
SetServoAngle(5, angle5);
SetServoAngle(6, angle6);
SetServoAngle(7, angle7);</p><p> SetServoAngle(TAIL_WIRE, TailAngle );
SetServoAngle(NECK_WIRE, NeckAngle);
SetServoAngle(HEAD_WIRE, HeadAngle);</p><p> //delay(25);</p><p>}</p><p>/***************************************************
MoveLegsForWalk()
************************************************* */
void MoveLegsForWalk(float StepPercentage)
{</p><p> LeftRightBalance(StepPercentage >= 1.0 ? -1: walkphase, xBalanceOffset);
FrontBackBalance(walkphase, -1);</p><p> FootXPos[0] = constrain(FootXPos[0], MIN_FOOT_X , MAX_FOOT_X);
FootXPos[1] = constrain(FootXPos[1], MIN_FOOT_X , MAX_FOOT_X);
FootXPos[2] = constrain(FootXPos[2], MIN_FOOT_X , MAX_FOOT_X);
FootXPos[3] = constrain(FootXPos[3], MIN_FOOT_X , MAX_FOOT_X);</p><p> FootYPos[0] = constrain(FootYPos[0], MIN_FOOT_HEIGHT , MAX_FOOT_HEIGHT);
FootYPos[1] = constrain(FootYPos[1], MIN_FOOT_HEIGHT , MAX_FOOT_HEIGHT);
FootYPos[2] = constrain(FootYPos[2], MIN_FOOT_HEIGHT , MAX_FOOT_HEIGHT);
FootYPos[3] = constrain(FootYPos[3], MIN_FOOT_HEIGHT , MAX_FOOT_HEIGHT);</p><p> MoveLegTo(FootXPos[0], FootYPos[0] + BACK_TO_FRONT_HIP_DIF, 0);
MoveLegTo(FootXPos[2], FootYPos[2] + BACK_TO_FRONT_HIP_DIF, 2);
MoveLegTo(FootXPos[1], FootYPos[1], 1);
MoveLegTo(FootXPos[3], FootYPos[3], 3);
}</p><p>/***************************************************
MoveTail()</p><p> relative angle is 0 is straight back
************************************************* */
void MoveTail()
{
TailPosition = constrain(TailPosition, TAIL_POSITION_MIN , TAIL_POSITION_MAX);
TailAngle = 90 + TailPosition + MotorBuildOffsets[8];
}</p><p>/***************************************************
MoveNeck()</p><p> relative angle is 0 is straight back
************************************************* */
void MoveNeck()
{
NeckPosition = constrain(NeckPosition, NECK_POSITION_MIN , NECK_POSITION_MAX);
NeckAngle = 90 + NeckPosition + MotorBuildOffsets[9];
}</p><p>/***************************************************
MoveHead()</p><p> relative angle is 0 is straight back
************************************************* */
void MoveHead()
{
HeadPosition = constrain(HeadPosition, HEAD_POSITION_MIN , HEAD_POSITION_MAX);
HeadAngle = 90 + HeadPosition + MotorBuildOffsets[9];
}</p><p>/***************************************************
StandPose()
************************************************* */
void StandPose()
{</p><p> speedIndex = 0;
xBalanceOffset = 0.0;</p><p> FootXPos[0] = 0; FootXPos[1] = 0; FootXPos[2] = 0; FootXPos[3] = 0;
FootYPos[0] = 92; FootYPos[1] = 92; FootYPos[2] = 92; FootYPos[3] = 92;</p><p> MoveLegTo(FootXPos[0], FootYPos[0] + BACK_TO_FRONT_HIP_DIF, 0);
MoveLegTo(FootXPos[2], FootYPos[2] + BACK_TO_FRONT_HIP_DIF, 2);
MoveLegTo(FootXPos[1], FootYPos[1], 1);
MoveLegTo(FootXPos[3], FootYPos[3], 3);</p><p> TailPosition = 0.0;
NeckPosition = 0.0;
HeadPosition = 0.0;</p><p> MoveTail();
MoveNeck();
MoveHead();
}</p><p>/***************************************************
SetFootXPosition
************************************************* */
void SetFootXPosition( int FootID, float StartPosition, float EndPosition, float LandFootSooner)
{</p><p> float HipOffset = 0.0;</p><p> if(FootID == 0 ||FootID == 2)
{
HipOffset = BACK_STEP_OFFSET_FROM_HIP_FORWARDS;</p><p> if(rightBackwards || leftBackwards)
HipOffset = BACK_STEP_OFFSET_FROM_HIP_BACKWARDS;
}</p><p> if(FootID == 1 ||FootID == 3)
{
HipOffset = FRONT_STEP_OFFSET_FROM_HIP_FORWARDS;</p><p> if(rightBackwards || leftBackwards)
HipOffset = FRONT_STEP_OFFSET_FROM_HIP_BACKWARDS;
}
unsigned long NowTime = millis();</p><p> float StepPercentage;
if(walkphase == FootID)
{
StepPercentage = (float)(NowTime - phaseStartTime[FootID]) / (StepTime[speedIndex] - LandFootSooner);
FootStanding[FootID] = false;
} else {
StepPercentage = (float)(NowTime - phaseStartTime[FootID]) / (StepTime[speedIndex] * 3);
}</p><p> StepPercentage = constrain(StepPercentage, 0.0 , 1.0);</p><p> if(!FootStanding[FootID]) {
// Most feet are moving backwards
float AdjustedEndPosition = -EndPosition;</p><p> // the foot that is lifted is moving forwards
if(walkphase == FootID)
AdjustedEndPosition = EndPosition;</p><p>
FootXPos[FootID] = GetRelativeValue( StartPosition, AdjustedEndPosition + HipOffset, StepPercentage);
}</p><p>}</p><p>/***************************************************
LeftRightBalance
************************************************* */
void LeftRightBalance(int UpFoot, float OffsetAngle)
{</p><p> float SavedFoot;
if (UpFoot >= 0)
SavedFoot = FootYPos[UpFoot];</p><p>
// This checks if we are standing. or not. That is the only time we will dynamically balance.
// It is possible to turn this on while walking, but need to tweak PID algorithm so not overcompensate.
if(OffsetAngle == 0.0 && mode == 1) {</p><p> float xoffset = mpu6050.getAngleX() + OffsetAngle;</p><p> //check if we just fell over
if (abs(xoffset) > 15)
return;
double Correction = CorrectSideLean(xoffset);
if (Correction < 0)
{
FootYPos[0] -= -Correction;
FootYPos[1] -= -Correction;
FootYPos[2] += -Correction;
FootYPos[3] += -Correction;
} else {
FootYPos[0] += Correction;
FootYPos[1] += Correction;
FootYPos[2] -= Correction;
FootYPos[3] -= Correction;
}
} else {</p><p> #define LEGS_WIDTH 80
double OffsetHeight = tan((OffsetAngle * 71.0)/ 4068.0) * LEGS_WIDTH/2;
FootYPos[0] = NORMAL_WALK_HEIGHT + OffsetHeight;
FootYPos[1] = NORMAL_WALK_HEIGHT + OffsetHeight;
FootYPos[2] = NORMAL_WALK_HEIGHT - OffsetHeight;
FootYPos[3] = NORMAL_WALK_HEIGHT - OffsetHeight;
}</p><p> if (UpFoot >= 0)
FootYPos[UpFoot] = SavedFoot;
}</p><p>/***************************************************
FrontBackBalance
************************************************* */
void FrontBackBalance(int UpFoot1, int UpFoot2)
{</p><p> float SavedFoot1;
if (UpFoot1 >= 0)
SavedFoot1 = FootYPos[UpFoot1];
float SavedFoot2;
if (UpFoot2 >= 0)
SavedFoot2 = FootYPos[UpFoot2];
// This checks if we are standing. or not. That is the only time we will dynamically balance.
// It is possible to turn this on while walking, but need to tweak PID algorithm so not overcompensate.
if(yBalanceOffset == 0.0 && mode == 1) {
float yoffset = mpu6050.getAngleY() + yBalanceOffset;
//check if we just fell over
if (abs(yoffset) > 15)
return;
double Correction = CorrectFrontBackLean(yoffset);</p><p> if (Correction < 0)
{
FootYPos[0] += -Correction;
FootYPos[2] += -Correction;
FootYPos[1] -= -Correction;
FootYPos[3] -= -Correction;
} else {
FootYPos[0] -= Correction;
FootYPos[2] -= Correction;
FootYPos[1] += Correction;
FootYPos[3] += Correction;
}</p><p> }</p><p> if (UpFoot1 >= 0)
FootYPos[UpFoot1] = SavedFoot1;
if (UpFoot2 >= 0)
FootYPos[UpFoot2] = SavedFoot2;
}</p><p>/***************************************************
GetRelativeValue
************************************************* */
float GetRelativeValue( float XStart, float XEnd, float StepPercentageDone)
{
return XStart + (XEnd - XStart) * StepPercentageDone;
}</p><p>/***************************************************
MoveLegTo</p><p> x,y is the position of the foot,
where the hip fo that foot is defined as 0,0 and positive y
is down (easier since hip is always above foot),
positive x is in front of the hip and negative x is behind.</p><p> LEGID
0 is back right
1 is front right
2 is back left
3 is front left</p><p> ************************************************* */
void MoveLegTo(float x, float y, int legID)
{</p><p> if (legID == 0 || legID == 2)
{ //These are back legs
float IK_A2 = IK_GetA2( x, y, BACK_LEG_L1, BACK_LEG_L2);
float IK_A1 = IK_GetA1( x, y, BACK_LEG_L1, BACK_LEG_L2, IK_A2);</p><p> float A1 = (IK_A1 * 57.2957795);
float A2 = (IK_A1 * 57.2957795) - (180 - (IK_A2 * 57.2957795));</p><p> if (legID == 0)
{
//back right leg
A1 = 180 - (A1 - BACK_LEG_HIP_OFFSET_ANGLE + 90 + MotorBuildOffsets[0]);
A2 = (A2 - BACK_LEG_KNEE_OFFSET_ANGLE + 90 + MotorBuildOffsets[1]);
angle0 = A1;
angle1 = A2;</p><p> } else {</p><p> //back left leg
A1 = (A1 - BACK_LEG_HIP_OFFSET_ANGLE + 90 + MotorBuildOffsets[4]);
A2 = 180 - (A2 - BACK_LEG_KNEE_OFFSET_ANGLE + 90 + MotorBuildOffsets[5]);
angle4 = A1;
angle5 = A2;</p><p> }
}
else
{
//These are the front legs</p><p> // The IK formula are assuming the knee bends forwards.
// So for the front leg I just invert X
x = -x;</p><p> float IK_A2 = IK_GetA2( x, y, FRONT_LEG_L1, FRONT_LEG_L2);
float IK_A1 = IK_GetA1( x, y, FRONT_LEG_L1, FRONT_LEG_L2, IK_A2);</p><p> float A1 = (IK_A1 * 57.2957795);
float A2 = (IK_A1 * 57.2957795) - (180 - (IK_A2 * 57.2957795));</p><p> if (legID == 1)
{
// Front right leg
A1 = (A1 - FRONT_LEG_HIP_OFFSET_ANGLE + 90 + MotorBuildOffsets[3]);
A2 = 180 - (A2 - FRONT_LEG_KNEE_OFFSET_ANGLE + 90 + MotorBuildOffsets [2]);
angle3 = A1;
angle2 = A2;</p><p> } else {</p><p> //front left leg
A1 = 180 - (A1 - FRONT_LEG_HIP_OFFSET_ANGLE + 90 + MotorBuildOffsets[7]);
A2 = (A2 - FRONT_LEG_KNEE_OFFSET_ANGLE + 90 + MotorBuildOffsets[6]);
angle7 = A1;
angle6 = A2;</p><p> }
}</p><p>}</p><p>/***************************************************
Inverse Kinematics for the legs</p><p> This is a generalized inverse kinematic function for
any two link leg. The length of the leg links are inputs.</p><p> A1 is the angle at the hip.
A2 is the angle at the knee
l1 is the length of the upper leg
l2 is the legnth of the lower leg
x,y is the position of the foot,
where the hip is defined as 0,0 and positive y
is down (easier since hip is always above foot).</p><p> For this robot all the lengths are millimeters, not that the code
cares, as along as all the length units are the same.</p><p>*******************************************/
float IK_GetA2(float x, float y, float l1, float l2)
{
return acos((x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2));
}</p><p>float IK_GetA1(float x, float y, float l1, float l2, float A2)
{
float IK_A1 = atan(y / x) - atan((l2 * sin(A2)) / (l1 + l2 * cos(A2)));</p><p> if (IK_A1 < 0)
IK_A1 += PI;</p><p> return IK_A1;
}</p><p>/*************************
CorrectSideLean
*************************/
double CorrectSideLean(double error)
{</p><p> double pTerm = Kp_LR * error;
double dTerm = Kd_LR * (error - last_error_LR);</p><p> integrated_error_LR += error;
double iTerm = Ki_LR * constrain(integrated_error_LR, -40, 40);
last_error_LR = error;</p><p> double PositionError = constrain(K_PLR * (pTerm + dTerm + iTerm), -GUARD_GAIN, GUARD_GAIN);</p><p> return PositionError;
}</p><p>/*************************
CorrectFrontBackLean
*************************/
double CorrectFrontBackLean(double error)
{</p><p> double pTerm = Kp_FB * error;
double dTerm = Kd_FB * (error - last_error_FB);</p><p> integrated_error_FB += error;
double iTerm = Ki_FB * constrain(integrated_error_FB, -40, 40);
last_error_FB = error;</p><p> double PositionError = constrain(K_PFB * (pTerm + dTerm + iTerm), -GUARD_GAIN, GUARD_GAIN);</p><p> return PositionError;
}</p><p>/***************************************************
Sets the pulse width of the servo controler given an angle</p><p>*******************************************/
int SetServoAngle(int servo, float angle)
{
int pulse_wide, analog_value;
pulse_wide = (angle * (float)(MAX_PULSE_WIDTH - MIN_PULSE_WIDTH)) / 180.0 + MIN_PULSE_WIDTH;
analog_value = (int)(((long)pulse_wide* (long)FREQUENCY * (long)4096) / (long)1000000) ;
pwm.setPWM(servo, 0, analog_value);</p><p> // delay(25);
}</p>Full Repo : https://github.com/JacquinBuchanan/Intellisaurus/t...