PDA

View Full Version : IssyDunnYet code issues



ArduTank
04-20-2014, 02:01 PM
Hello All

I am trying to use the IssyDunnYet code again, and I'm still running into the same issue with not being able to get him to turn, just walk forwards/backwards.

I've played around with setting register addresses correctly and changing axis' on my xb360 controller for use with it.

However, I still get one iteration of "Packet Error" from the 'bot, and absence of turning control.

What I am asking is for you guys to look at the attached code and see if any of you guys (who know more dealing with Python, Wiring, and packets than I do) can see any reason why it would not be working.

Hate having to ask for help, but I have no clue what is not working right here.

Thanks for your time,

ArduTank


PyMechCon:


#!/usr/bin/env python

"""
PyMech Controller Ver A.0

Copyright (c) 2009 Michael E. Ferguson. All right reserved.

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software Foundation,
Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA

http://www.blunderingbotics.com/

pyMechCon is a python class for controlling an AVR-XBEE powered
mech warrior. It provides control over a mech's forward speed
and radius of rotation, as well as torso twist, and control of
up to 4 guns, in rapid-all-fire mode or a rotating burst mode.

Configuration is put into pyMechConf.py, you can automatically
create this file by running ConfigureJoystick.py

All values sent to controller are -100 to 100.
"""

import pygame, time, sys, serial, threading
from pygame.locals import *
from pyMechConf import *

# register definitions in AVR
REG_FORWARD = 0x10
REG_TURN = 0x11
REG_PAN = 0x12
REG_TILT = 0x13
REG_FIRE = 0x14
REG_STATUS = 0x20

class pyMechCon:
def __init__(self, ser=None, port="COM3", baud=38400):
# parameters we send
self.forward = 0 # forward speed
self.turn = 0 # turn speed
self.pan = 0 # pan servo position
self.tilt = 0 # tilt servo position
self.fire = 0 # which guns are firing

self.adjust = 0 # how much to turn
self.TURRET_ACTIVE = False # do we move torso or body?
self.exitcond = False

if ser != None:
self.ser = ser
else:
try:
self.ser = serial.Serial()
self.ser.baudrate = baud
self.ser.port = port
self.ser.timeout = 3
self.ser.open()
print "pyMechCon: Port Opened"
except:
print "pyMechCon: Cannot open port"
sys.exit(0)
# we want a joystick (we should also have an on screen way to control the mech?)
self.stick = pygame.joystick.Joystick(0)
self.stick.init()

def sendPacket(self, reg, data):
self.ser.write('\xFF')
self.ser.write(chr(reg))
self.ser.write(chr(1))
self.ser.write(chr(data + 128))
self.ser.write(chr(255 - ((data + 129 + reg)%256)))

def sendFullPacket(self):
self.ser.write('\xFF')
self.ser.write(chr(REG_FORWARD))
self.ser.write(chr(5))
self.ser.write(chr(self.forward + 128))
self.ser.write(chr(self.turn + 128))
self.ser.write(chr(self.pan + 128))
self.ser.write(chr(self.tilt + 128))
self.ser.write(chr(self.fire + 128))
self.ser.write(chr(255 - ((self.forward + self.turn + self.pan + self. tilt + self.fire + 128 + 5 + REG_FORWARD)%256)))
return "pyMechCon: Status = " , str(int(self.forward)) , str(int(self.turn))# , str(int(self.servo))

def thread(self):
""" This is the seperate thread, for handling joystick control """
while not self.exitcond:
for event in pygame.event.get():
self.handle(event)
# now send data!
self.sendFullPacket()
time.sleep(0.033333)

def handle(self, event):
""" Actually do something with the event. """
if event.type == pygame.QUIT:
# send quit to serial thread?
self.exitcond = True
sys.exit()
elif event.type == JOYAXISMOTION:
if event.axis == FORWARD_AXIS:
# user has pushed forward on joystick
if self.TURRET_ACTIVE:
self.tilt = int(-50 * event.value) # was only 100s
else:
self.forward = int(-50 * event.value) #50 for final # was 100
elif event.axis == TURN_AXIS:
# joystick has moved to the side
if self.TURRET_ACTIVE:
self.pan = int(100 * event.value)
else:
if self.forward > 25:
self.turn = int(self.adjust * event.value * 0.25)
if self.forward >= -10:
self.turn = int(self.adjust * event.value)
else:
self.turn = -int(self.adjust * event.value * 0.25)
#print self.turn
elif event.axis == ADJUST_AXIS:
self.adjust = -15 * (event.value - 1)
print "Adjustment value: " + str(self.adjust)
elif event.type == JOYBUTTONDOWN:
if event.button == BUTTON_SERVO:
self.TURRET_ACTIVE = True
elif event.button == BUTTON_SERVO_CENTER:
self.pan = 0
self.tilt = 0
elif event.button == BUTTON_SERVO_LEFT:
self.pan = self.pan + 3
elif event.button == BUTTON_SERVO_RIGHT:
self.pan = self.pan - 3
elif event.button == BUTTON_STAND:
print "stand"
self.sendPacket(REG_STATUS, 1) #-127)
elif event.button == BUTTON_RELAX:
print "relax"
self.sendPacket(REG_STATUS, -1) #-128)
elif event.button == BUTTON_FIRE:
print "fire"
self.fire = 1
elif event.type == JOYBUTTONUP:
if event.button == BUTTON_SERVO:
self.TURRET_ACTIVE = False
self.pan = 0 # do we want to do this?
#self.tilt = 0
elif event.button == BUTTON_FIRE:
print "end fire"
self.fire = 0

if __name__ == "__main__":
pygame.init()
pyCon = pyMechCon()
t = threading.Thread(target=pyCon.thread)
t.start()
try:
while True:
while pyCon.ser.inWaiting > 0:
s = pyCon.ser.readline()
print s,
time.sleep(0.01)
except KeyboardInterrupt:
sys.exit(0)






Issy.ino (code on the ArbotiX):


/*
IssyDunnYet reworked code for arbotiX controller using Sanguino environment.
This sketch will allow you to drive around a 4-legged, 3DOF per leg, quad, using
the PyMech program.

Copyright (c) 2008-2012 Michael E. Ferguson. All right reserved.

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/

#include <Motors2.h>
#include <ax12.h>
#include <BioloidController.h>
#include "poses.h"

BioloidController bioloid = BioloidController(1000000);
Motors2 guns = Motors2();

#define VOLTAGE_READ 7

// servo numbers
#define RIGHT_FRONT 1
#define LEFT_FRONT 3
#define RIGHT_REAR 8
#define LEFT_REAR 2
#define TILT_SERVO 13

// we use a register based write algorithm, similar to the Dynamixel bus.
#define REG_FORWARD 0x10
#define REG_TURN 0x11 // -128..128
#define REG_PAN 0x12 // turret pan servo, value in degrees, 0=forward, -90=left
#define REG_TILT 0x13
#define REG_FIRE 0x14
#define REG_STATUS 0x20

int mode = 0; // where are we in our walk cycle?
int addr = 0; // what register address are we writing to?
int count = 0;
int checksum = 0;
int iter = 0;

/* temporary holders during a register packet write. */
int nForward = 0;
int nTurn = 0;
int nPan = 0;
int nTilt = 0;
int nFire = 0;
int nStatus = 0;
unsigned char data;

/* Movement parameters */
int rstep, lstep;
int pan, tilt;
int walkMode; // where are we in our step sequence of 0-3, -1 = relaxed

/* Power variables */
int vmain = 1200;
unsigned long vtime = 0; // last time we sent out voltages

void setup(){
int i;
Serial.begin(38400);
pinMode(0,OUTPUT);
delay(1000);

// stand up slowly
bioloid.loadPose(walkend);
bioloid.readPose();
bioloid.interpolateSetup(1500); // from current position to standing in 1.5 seconds
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
}
delay(500);

Serial.println("Issy Alive");
// walking parameters
walkMode = 0;
rstep = 0;
lstep = 0;
mode = 0;
}

/* main loop, read in serial data and walk. */
void loop(){
int i;

// process messages coming in from PyMech
while(Serial.available() > 0){
// We need to 0xFF at start of packet
if(mode == 0){ // start of new packet
if(Serial.read() == 0xff)
mode = 1;
digitalWrite(0,HIGH-digitalRead(0));
}else if(mode == 1){ // next byte is address
addr = (int) Serial.read();
if(addr != 0xff)
mode = 2;
}else if(mode == 2){ // next byte is byte count to write
count = (int) Serial.read();
checksum = addr + count;
iter = 0;
mode = 3;
}else if(mode == 3){ // read data in
data = (unsigned char) Serial.read();
checksum += (int) data;
if(iter == count){
// process request
if((checksum%256) != 255){
Serial.println("Packet Error");
}else{
// deal with status/relax updates
if(nStatus > 0){
bioloid.loadPose(walkend); //stand);
bioloid.writePose();
nStatus = 0;
}else if(nStatus < 0){
bioloid.interpolating = 0;
for(data=0;data<12;data++)
Relax(data);
lstep=0;rstep=0;
nStatus = 0;
}else{ // if not paused, or just standing, lets update stuff
lstep = nForward+nTurn;
rstep = nForward-nTurn;
// issy has no pan servo, instead we shift the body
if(nPan != 0){ // note that shifting only works when not walking.
bioloid.loadPose(walkend);
// left front back, rightfront forward
bioloid.setNextPose(LEFT_FRONT, bioloid.getNextPose(LEFT_FRONT) + nPan);
bioloid.setNextPose(RIGHT_FRONT, bioloid.getNextPose(RIGHT_FRONT) + nPan);
// left rear back, right rear back
bioloid.setNextPose(LEFT_REAR, bioloid.getNextPose(LEFT_REAR) + nPan);
bioloid.setNextPose(RIGHT_REAR, bioloid.getNextPose(RIGHT_REAR) + nPan);
bioloid.writePose();
}
SetPosition(TILT_SERVO, 512 + nTilt*2);
}
// fire guns, or stop firing
if(nFire > 0) guns.right(170);
else guns.right(0);
}
mode = 0;
}else{
if(addr == REG_FORWARD) nForward = (int) (data-128);
else if(addr == REG_TURN) nTurn = (int) (data-128);
else if(addr == REG_PAN) nPan = (int) (data-128);
else if(addr == REG_TILT) nTilt = (int) (data-128);
else if(addr == REG_FIRE) nFire = (int) (data-128);
else if(addr == REG_STATUS) nStatus = (int) (data-128);
}
iter++;addr++;
}
}

// update parameters
if(bioloid.interpolating == 0){
if((rstep != 0) || (lstep != 0)){ // do nothing if we are stopped
if(walkMode == 0){
bioloid.loadPose(walk0);
bioloid.setNextPose(RIGHT_FRONT,bioloid.getNextPos e(RIGHT_FRONT) - rstep);
bioloid.setNextPose(LEFT_FRONT,bioloid.getNextPose (LEFT_FRONT) - lstep);
bioloid.setNextPose(RIGHT_REAR,bioloid.getNextPose (RIGHT_REAR) + rstep);
bioloid.setNextPose(LEFT_REAR,bioloid.getNextPose( LEFT_REAR) + lstep);
walkMode = 1;
bioloid.interpolateSetup(75);
}else if(walkMode == 1){
bioloid.loadPose(walkend);
bioloid.setNextPose(RIGHT_FRONT,bioloid.getCurPose (RIGHT_FRONT));
bioloid.setNextPose(LEFT_FRONT,bioloid.getCurPose( LEFT_FRONT));
bioloid.setNextPose(RIGHT_REAR,bioloid.getCurPose( RIGHT_REAR));
bioloid.setNextPose(LEFT_REAR,bioloid.getCurPose(L EFT_REAR));
walkMode = 2;
bioloid.interpolateSetup(30);
}else if(walkMode == 2){
bioloid.loadPose(walk1);
bioloid.setNextPose(RIGHT_FRONT,bioloid.getNextPos e(RIGHT_FRONT) + rstep);
bioloid.setNextPose(LEFT_FRONT,bioloid.getNextPose (LEFT_FRONT) + lstep);
bioloid.setNextPose(RIGHT_REAR,bioloid.getNextPose (RIGHT_REAR) - rstep);
bioloid.setNextPose(LEFT_REAR,bioloid.getNextPose( LEFT_REAR) - lstep);
walkMode = 3;
bioloid.interpolateSetup(75);
}else{
bioloid.loadPose(walkend);
bioloid.setNextPose(RIGHT_FRONT,bioloid.getCurPose (RIGHT_FRONT));
bioloid.setNextPose(LEFT_FRONT,bioloid.getCurPose( LEFT_FRONT));
bioloid.setNextPose(RIGHT_REAR,bioloid.getCurPose( RIGHT_REAR));
bioloid.setNextPose(LEFT_REAR,bioloid.getCurPose(L EFT_REAR));
walkMode = 0;
bioloid.interpolateSetup(30);
}
}else{
// stopped! put feet down!
bioloid.loadPose(walkend);
bioloid.setNextPose(RIGHT_FRONT,bioloid.getCurPose (RIGHT_FRONT));
bioloid.setNextPose(LEFT_FRONT,bioloid.getCurPose( LEFT_FRONT));
bioloid.setNextPose(RIGHT_REAR,bioloid.getCurPose( RIGHT_REAR));
bioloid.setNextPose(LEFT_REAR,bioloid.getCurPose(L EFT_REAR));
bioloid.interpolateSetup(30);
}
}

// voltage monitoring loop
//if((millis() - vtime) > 3000){
// int v = analogRead(VOLTAGE_READ);
// convert V to voltage (since it is scaled), and then add to running average
// vmain = ((vmain * 5)/10) + ((((v * 14)/10) * 5)/10);
// if(vmain < 980){ // 9.8V is fairly conservative, but shutdown!
// Serial.println("Battery Expired!");
// guns.right(0); // kill guns
// for(mode=0;mode<13;mode++) // kill servos
// Relax(mode);
// while(1);
// }else{ // print out voltage
// float x = vmain/100;
// Serial.print("Voltage:");
// Serial.println(x);
// vtime = millis();

// handle temp readback
// for(v=1;v<13;v++){
// int data = ax12GetRegister(v, AX_PRESENT_TEMPERATURE, 1);
// Serial.print("S");
// Serial.print((v/100));
// Serial.print((v/10)%10);
// Serial.print(v%10);
// Serial.print("TEMP:");
// Serial.print((data/100));
// Serial.print((data/10)%10);
// Serial.println(data%10);



// update joints
bioloid.interpolateStep();
}

ArduTank
04-20-2014, 08:16 PM
Hello all. Little update, found that I may be having an issue with the 'adjustment' value, not any code mis-communication.

So, my previous post is irrelevant. I would appreciate it if a mod would lock this thread.