Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
204 lines (158 sloc) 4.56 KB
#include <Servo.h>
/*
* Auto-Generated by NUKE!
* http://arbotix.googlecode.com
*/
#include <ax12.h>
#include <BioloidController.h>
#include <Commander.h>
#include "nuke.h"
// Define one or the other depending upon which servo type you are using.
#define AX12_HEXAPOD
//#define AX18_HEXAPOD
Commander command = Commander();
int multiplier;
Servo pan;
Servo tilt;
int panValue = 90;
int tiltValue = 90;
int vericalMapped;
int horizontalMapped;
#define RIPPLE_SPEED 1
#define AMBLE_SPEED 3
#define TRIPOD_SPEED 5
#ifdef AX12_HEXAPOD
#define TOP_SPEED 10
#endif
#ifdef AX18_HEXAPOD
#define TOP_SPEED 12
#endif
int speed = 5;
void setup(){
pinMode(0,OUTPUT);
pinMode(7,OUTPUT);
// setup IK
setupIK();
gaitSelect(AMBLE_SMOOTH);
// setup serial
Serial.begin(38400);
// wait, then check the voltage (LiPO safety)
delay (1000);
float voltage = (ax12GetRegister (1, AX_PRESENT_VOLTAGE, 1)) / 10.0;
Serial.print ("System Voltage: ");
Serial.print (voltage);
Serial.println (" volts.");
if (voltage < 10.0)
while(1);
// stand up slowly
bioloid.poseSize = 18;
bioloid.readPose();
doIK();
bioloid.interpolateSetup(1000);
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
}
multiplier = AMBLE_SPEED;
pan.attach(15);
tilt.attach(14);
pan.write(0);
tilt.write(90);
}
void loop(){
// take commands
if(command.ReadMsgs() > 0){
if(command.buttons&BUT_RT)
{
if((command.walkV) > 5 || (command.walkV < -5) ){
vericalMapped = map(command.walkV, -128, 128, speed, -speed); //Map analog value from native joystick value (0 to 1023) to incremental change (speed to -speed)
tiltValue = tiltValue -vericalMapped;
tiltValue = max(tiltValue, 0);//use the max() function to make sure the value never falls below TILT_MIN(0 degrees)
tiltValue = min(tiltValue, 180);//use the min() function to make sute the value never goes above TILT_MAX (180 degrees)
tilt.write(tiltValue);
}
if((command.walkH) > 5 || (command.walkH < -5) ){
horizontalMapped = map(command.walkH, -128, 128, speed, -speed); //Map analog value from native joystick value (0 to 1023) to incremental change (speed to -speed)
panValue = panValue + horizontalMapped;
panValue = max(panValue, 0);//use the max() function to make sure the value never falls below TILT_MIN(0 degrees)
panValue = min(panValue, 180);//use the min() function to make sute the value never goes above TILT_MAX (180 degrees)
pan.write(panValue);
}
}
else
{
digitalWrite(0,HIGH-digitalRead(0));
// select gaits
if(command.buttons&BUT_R1){
gaitSelect(RIPPLE_SMOOTH);
multiplier=RIPPLE_SPEED;
}
if(command.buttons&BUT_R2){
gaitSelect(AMBLE_SMOOTH);
multiplier=AMBLE_SPEED;
}
if(command.buttons&BUT_R3){
gaitSelect(RIPPLE);
multiplier=RIPPLE_SPEED;
}
if(command.buttons&BUT_L4){
gaitSelect(AMBLE);
multiplier=AMBLE_SPEED;
}
if(command.buttons&BUT_L5){
gaitSelect(TRIPOD);
multiplier=TRIPOD_SPEED;
}
if(command.buttons&BUT_L6){
gaitSelect(TRIPOD);
multiplier=TOP_SPEED;
}
if(command.buttons&BUT_LT){
digitalWrite(7,HIGH);
}
else
{
digitalWrite(7,LOW);
}
// set movement speed
if((command.walkV) > 5 || (command.walkV < -5) ){
Xspeed = (multiplier*command.walkV)/2;
}
else
{
Xspeed = 0;
}
if((command.walkH) > 5 || (command.walkH < -5) ){
Yspeed = (multiplier*command.walkH)/2;
}
else
{
Yspeed = 0;
}
if((command.lookH) > 5 || (command.lookH < -5) ){
Rspeed = -(command.lookH)/100.0;
}
else
{
Rspeed = 0;
}
}
// Use the phoenix code if you want pretty body rotation. :)
//
// if((command.buttons&BUT_LT) > 0){
// bodyRotY = (((float)command.lookV))/300.0;
// bodyRotZ = ((float)command.lookH)/300.0;
// bodyRotX = ((float)command.walkH)/300.0;
// Rspeed = 0;
// Xspeed = 0;
// Yspeed = 0;
// }
}
// if our previous interpolation is complete, recompute the IK
if(bioloid.interpolating == 0){
doIK();
bioloid.interpolateSetup(tranTime);
}
// update joints
bioloid.interpolateStep();
}
You can’t perform that action at this time.