Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 157 → Rev 158

/avr/bioloid-gripper/bioloidGripper.h
0,0 → 1,102
/****************************************************************************
*
* Copyright (c) 2007 Jon Hylands <jon@huv.com>
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* Alternatively, this software may be distributed under the terms of BSD
* license.
*
* See README and COPYING for more details.
*
****************************************************************************/
/**
*
* @file bioloidFoot.h
*
* @brief Contains definitions for a bioloid Foot Pressure sensor.
*
****************************************************************************/
 
#if !defined( BIOLOID_GRIPPER_H )
#define BIOLOID_GRIPPER_H /**< Include Guard */
 
#define TRUE 1
#define FALSE 0
 
#define BLD_STATUS_RETURN_LEVEL_NONE 0 // Don't respond
#define BLD_STATUS_RETURN_LEVEL_READ_DATA 1 // respond only to READ_DATA
#define BLD_STATUS_RETURN_LEVEL_ALL 2 // Respond to everything
 
#define EEPROM_BASE_ADDRESS 11
#define EEPROM_ELEMENT_COUNT 7
 
#define CONTROL_MODEL_NUMBER_LOW 0
#define CONTROL_MODEL_NUMBER_HIGH 1
#define CONTROL_FIRMWARE_VERSION 2
#define CONTROL_ID 3
#define CONTROL_BAUD_RATE 4
#define CONTROL_RETURN_DELAY_TIME 5
 
#define CONTROL_FINGER_KP_LOW 6
#define CONTROL_FINGER_KP_HIGH 7
#define CONTROL_FINGER_KD_LOW 8
#define CONTROL_FINGER_KD_HIGH 9
#define CONTROL_FINGER_KI_LOW 10
#define CONTROL_FINGER_KI_HIGH 11
 
#define CONTROL_STATUS_RETURN_LEVEL 16
 
#define CONTROL_WRIST_KP_LOW 17
#define CONTROL_WRIST_KP_HIGH 18
#define CONTROL_WRIST_KD_LOW 19
#define CONTROL_WRIST_KD_HIGH 20
#define CONTROL_WRIST_KI_LOW 21
#define CONTROL_WRIST_KI_HIGH 22
 
#define MAX_EEPROM_ENTRY 23
 
#define CONTROL_LED 25
#define CONTROL_DESIRED_FINGER_POSITION_LOW 26
#define CONTROL_DESIRED_FINGER_POSITION_HIGH 27
#define CONTROL_DESIRED_FINGER_SPEED_LOW 28
#define CONTROL_DESIRED_FINGER_SPEED_HIGH 29
#define CONTROL_FINGER_POSITION_LOW 30
#define CONTROL_FINGER_POSITION_HIGH 31
#define CONTROL_FINGER_PRESSURE_LOW 32
#define CONTROL_FINGER_PRESSURE_HIGH 33
#define CONTROL_DESIRED_WRIST_POSITION_LOW 34
#define CONTROL_DESIRED_WRIST_POSITION_HIGH 35
#define CONTROL_DESIRED_WRIST_SPEED_LOW 36
#define CONTROL_DESIRED_WRIST_SPEED_HIGH 37
#define CONTROL_WRIST_POSITION_LOW 38
#define CONTROL_WRIST_POSITION_HIGH 39
 
#define NUM_CONTROL_ENTRIES 40
#define MAX_CONTROL_ENTRY 39
 
#define GRIPPER_PRESSURE_CHANNEL 2
#define GRIPPER_POSITION_CHANNEL 7
#define GRIPPER_ROTATION_CHANNEL 3
 
#define WRIST_ROTATION_STOPPED 0
#define WRIST_ROTATION_FORWARDS 1
#define WRIST_ROTATION_BACKWARDS 2
 
#define WRIST_ROTATION_THRESHOLD 500
#define WRIST_ROTATION_BLACK 1
#define WRIST_ROTATION_WHITE 0
 
#define WRIST_ROTATION_TICK_ANGLE 10
 
#define MIN_FINGER_POSITION 65
#define MAX_FINGER_POSITION 540
#define FINGER_MOTOR_DEADBAND 60
#define FINGER_POSITION_DEADBAND 10
 
#define WRIST_MOTOR_DEADBAND 50
#define WRIST_POSITION_DEADBAND 10
 
#endif /* BIOLOID_GRIPPER_H */
/avr/bioloid-gripper/Motor.c
0,0 → 1,109
/****************************************************************************
*
* Motor.c
*
* Motor Control Code
*
****************************************************************************/
 
// ---- Include Files -------------------------------------------------------
 
#include <avr/io.h>
 
#include "Motor.h"
 
// ---- Public Variables ----------------------------------------------------
// ---- Private Constants and Types -----------------------------------------
 
// Port B
 
enum
{
MotorA_PWM = 1, // PWM Wire - B1
MotorA_Enable = 7, // Enable Wire - B7
 
MotorB_PWM = 2, // PWM Wire - B2
MotorB_Enable = 6, // Enable Wire - B6
};
 
// ---- Private Variables ---------------------------------------------------
// ---- Private Function Prototypes -----------------------------------------
// ---- Functions -----------------------------------------------------------
 
extern void InitMotors( void )
{
// 8 bit PWM yields divide by 255
 
#define CHAN_A_NON_INVERTING_PWM ( 1 << COM1A1 ) | ( 0 << COM1A0 )
#define CHAN_B_NON_INVERTING_PWM ( 1 << COM1B1 ) | ( 0 << COM1B0 )
#define PWM_8_BIT_MODE ( 0 << WGM11 ) | ( 1 << WGM10 )
 
TCCR1A = CHAN_A_NON_INVERTING_PWM | CHAN_B_NON_INVERTING_PWM | PWM_8_BIT_MODE;
 
#define T1_PRESCALAR_1 ( 0 << CS22 ) | ( 0 << CS21 ) | ( 1 << CS20 )
#define T1_PRESCALAR_8 ( 0 << CS22 ) | ( 1 << CS21 ) | ( 0 << CS20 )
 
TCCR1B = T1_PRESCALAR_1; // Divide by 1 prescalar
 
OCR1A = 0;
OCR1B = 0;
 
// Set the 4 motor control pins as outputs
 
{
uint8_t pins = ( 1 << MotorA_Enable ) | ( 1 << MotorA_PWM )
| ( 1 << MotorB_Enable ) | ( 1 << MotorB_PWM );
PORTB |= pins;
DDRB |= pins;
}
 
SetMotorSpeed( SPEED_OFF, SPEED_OFF );
}
 
/****************************************************************************
*
* Sets the motor speed of both motors.
*/
 
extern void SetMotorSpeed( speed_t speedWrist, speed_t speedFinger )
{
static speed_t prevSpeedWrist = SPEED_OFF + 1;
static speed_t prevSpeedFinger = SPEED_OFF + 1;
 
if ( speedWrist != prevSpeedWrist )
{
if (speedWrist == SPEED_OFF)
PORTB |= ( 1 << MotorA_Enable );
else
PORTB &= ~( 1 << MotorA_Enable );
OCR1A = speedWrist;
prevSpeedWrist = speedWrist;
}
 
if ( speedFinger != prevSpeedFinger )
{
if (speedFinger == SPEED_OFF)
PORTB |= ( 1 << MotorB_Enable );
else
PORTB &= ~( 1 << MotorB_Enable );
OCR1B = speedFinger;
prevSpeedFinger = speedFinger;
}
 
} // SetMotorSpeed
 
extern void disableMotor( void )
{
PORTB |= ( 1 << MotorA_Enable );
PORTB |= ( 1 << MotorB_Enable );
} // Disable motor
 
extern void enableMotor( void )
{
PORTB &= ~( 1 << MotorA_Enable );
PORTB &= ~( 1 << MotorB_Enable );
} // Enable motor
 
 
 
/avr/bioloid-gripper/Motor.h
0,0 → 1,36
/****************************************************************************
*
* Motor.h
*
* Motor Control Code
*
****************************************************************************/
 
#if !defined( MOTOR_H )
#define MOTOR_H
 
#include <inttypes.h>
 
// Motor Speeds are defined as a percentage
 
#define SPEED_OFF 127
 
typedef uint16_t speed_t;
 
void InitMotors( void );
void SetMotorSpeed( speed_t speedLeft, speed_t speedRight );
void disableMotor( void );
void enableMotor( void );
 
#endif // MOTOR_H
 
 
 
 
 
 
 
 
 
 
 
/avr/bioloid-gripper/Config.h
0,0 → 1,51
/****************************************************************************
*
* Copyright (c) 2006 Dave Hylands <dhylands@gmail.com>
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* Alternatively, this software may be distributed under the terms of BSD
* license.
*
* See README and COPYING for more details.
*
****************************************************************************/
 
#if !defined( CONFIG_H )
#define CONFIG_H
 
#define CFG_TIMER_UART_TX_BUFFER_SIZE 64
#define CFG_TIMER_UART_BAUD_RATE 19200
#define CFG_TIMER_UART_LF_TO_CRLF 1
#define CFG_TIMER_UART_DDR DDRD
#define CFG_TIMER_UART_PORT PORTD
#define CFG_TIMER_UART_MASK ( 1 << 4 )
 
#define CFG_LOG_TO_BUFFER 1
#define CFG_LOG_NUM_BUFFER_ENTRIES 32
#define CFG_LOG_USE_STDIO 0
#define CFG_LOG_PUT_CHAR_FUNC TimerUART_PutChar
#define CFG_LOG_PUT_CHAR_HDR "Timer-UART.h"
 
#define CFG_ADC_8_BIT 0
#define CFG_ADC_POLL 0
#define CFG_ADC_AVERAGE 1
 
//#define CFG_USE_UART0 1
//#define CFG_UART0_BAUD_RATE 1000000
//#define CFG_UART0_RX_BUFFER_SIZE 128
//#define CFG_UART0_TX_BUFFER_SIZE 128
//#define CFG_UART0_LF_TO_CRLF 1
 
#define CFG_BLD_USE_UART0 1
#define CFG_BLD_UART0_BAUD_RATE 1000000
#define CFG_BLD_UART0_RX_BUFFER_SIZE 64
#define CFG_BLD_UART0_TX_BUFFER_SIZE 64
#define CFG_BLD_UART0_RCV_TX 0
#define CFG_BLD_UART0_ENABLE_TX_PULLUP 0
 
#endif // CONFIG_H
 
 
/avr/bioloid-gripper/bioloidGripper.c
0,0 → 1,698
/****************************************************************************
*
* Copyright (c) 2007 Jon Hylands <jon@huv.com>
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* Alternatively, this software may be distributed under the terms of BSD
* license.
*
* See README and COPYING for more details.
*
****************************************************************************/
 
/* ---- Include Files ---------------------------------------------------- */
 
#include <avr/io.h>
#include <stdio.h>
#include <string.h>
#include <avr/eeprom.h>
#include <stdlib.h>
#include <math.h>
 
#include "Delay.h"
#include "adc.h"
#include "Log.h"
#include "Timer.h"
#include "Timer-UART.h"
#include "bioloid-uart.h"
#include "bioloid-pkt.h"
#include "bioloidGripper.h"
#include "Motor.h"
 
/* ---- Public Variables ------------------------------------------------- */
 
/* ---- Private Constants and Types -------------------------------------- */
 
// Our LED is connected to PD3
 
#define LED_DDR DDRD
#define LED_PORT PORTD
#define LED_MASK (1 << 3)
 
/* ---- Private Variables ------------------------------------------------ */
 
uint8_t gMyId;
uint8_t gControlTable [NUM_CONTROL_ENTRIES];
 
uint8_t gWristRotationDirection = WRIST_ROTATION_STOPPED;
double gWristRotationIncrement;
double gWristRotationPosition;
 
// Gripper Finger Variables.
int16_t lastFingerOffset = 0;
int16_t fingerErrorAccumulation = 0;
double fPk = 0.3, fDk = 0.0, fIk = 0.0; // PID constants
int16_t speedOffset = 512;
 
// Gripper Wrist Variables.
int16_t lastWristOffset = 0;
int16_t wristErrorAccumulation = 0;
double wPk = 0.2, wDk = 0.0, wIk = 0.0; // PID constants
 
uint8_t gCycleCount;
uint8_t sensorCycleCount = 0;
/* ---- Functions -------------------------------------------------------- */
 
 
//***************************************************************************
/**
* The baud rate has been changed. Update UART0
*/
 
static void HandleBaudRateChange (uint8_t baudRate)
{
switch (baudRate)
{
case 1: UART0_SetBaudRate (1000000); break;
case 3: UART0_SetBaudRate (500000); break;
case 4: UART0_SetBaudRate (400000); break;
case 7: UART0_SetBaudRate (250000); break;
case 9: UART0_SetBaudRate (200000); break;
case 16: UART0_SetBaudRate (115200); break;
case 34: UART0_SetBaudRate (57600); break;
case 103: UART0_SetBaudRate (19200); break;
case 207: UART0_SetBaudRate (9600); break;
}
}
 
 
//***************************************************************************
/**
* Update the EEPROM with what's in memory...
*/
 
static void UpdateEEPROM (void)
{
Log ("Writing EEPROM Control Table Entries...\n");
// We keep this magic constant in (EEPROM_BASE_ADDRESS - 1) as a form of CRC
eeprom_write_byte ((uint8_t*)(EEPROM_BASE_ADDRESS - 1), EEPROM_ELEMENT_COUNT);
eeprom_write_byte ((uint8_t*)(EEPROM_BASE_ADDRESS + CONTROL_MODEL_NUMBER_LOW), gControlTable [CONTROL_MODEL_NUMBER_LOW]);
eeprom_write_byte ((uint8_t*)(EEPROM_BASE_ADDRESS + CONTROL_MODEL_NUMBER_HIGH), gControlTable [CONTROL_MODEL_NUMBER_HIGH]);
eeprom_write_byte ((uint8_t*)(EEPROM_BASE_ADDRESS + CONTROL_FIRMWARE_VERSION), gControlTable [CONTROL_FIRMWARE_VERSION]);
eeprom_write_byte ((uint8_t*)(EEPROM_BASE_ADDRESS + CONTROL_ID), gControlTable [CONTROL_ID]);
eeprom_write_byte ((uint8_t*)(EEPROM_BASE_ADDRESS + CONTROL_BAUD_RATE), gControlTable [CONTROL_BAUD_RATE]);
eeprom_write_byte ((uint8_t*)(EEPROM_BASE_ADDRESS + CONTROL_RETURN_DELAY_TIME), gControlTable [CONTROL_RETURN_DELAY_TIME]);
eeprom_write_byte ((uint8_t*)(EEPROM_BASE_ADDRESS + CONTROL_STATUS_RETURN_LEVEL), gControlTable [CONTROL_STATUS_RETURN_LEVEL]);
}
 
 
//***************************************************************************
/**
* Initialize Control Table
*/
 
static void InitializeControlTable (uint8_t forceOverwrite)
{
uint8_t eepromSize;
 
memset (&gControlTable, 0, sizeof (gControlTable));
 
// NOTE - These are the hardware defaults...
gControlTable [CONTROL_MODEL_NUMBER_LOW] = 12;
gControlTable [CONTROL_MODEL_NUMBER_HIGH] = 67;
gControlTable [CONTROL_FIRMWARE_VERSION] = 1;
gControlTable [CONTROL_ID] = 80;
gControlTable [CONTROL_BAUD_RATE] = 1;
gControlTable [CONTROL_RETURN_DELAY_TIME] = 250;
gControlTable [CONTROL_STATUS_RETURN_LEVEL] = BLD_STATUS_RETURN_LEVEL_ALL;
 
gControlTable [CONTROL_DESIRED_FINGER_POSITION_LOW] = (MAX_FINGER_POSITION & 0xFF);
gControlTable [CONTROL_DESIRED_FINGER_POSITION_HIGH] = (MAX_FINGER_POSITION >> 8);
gControlTable [CONTROL_DESIRED_FINGER_SPEED_LOW] = 255;
gControlTable [CONTROL_DESIRED_FINGER_SPEED_HIGH] = 3;
 
gControlTable [CONTROL_DESIRED_WRIST_POSITION_LOW] = 0;
gControlTable [CONTROL_DESIRED_WRIST_POSITION_HIGH] = 2;
gControlTable [CONTROL_DESIRED_WRIST_SPEED_LOW] = 255;
gControlTable [CONTROL_DESIRED_WRIST_SPEED_HIGH] = 3;
 
// if the magic constant is there, the rest of the values are probably okay.
// if its not there, or we're doing a hard reset, ignore what is in the EEPROM,
// and over-write it with the hardware default values...
eepromSize = eeprom_read_byte ((uint8_t*)(EEPROM_BASE_ADDRESS - 1));
if (!forceOverwrite && (eepromSize == EEPROM_ELEMENT_COUNT))
{
Log ("Reading EEPROM Control Table Entries...\n");
gControlTable [CONTROL_MODEL_NUMBER_LOW] = eeprom_read_byte ((uint8_t*)(EEPROM_BASE_ADDRESS + CONTROL_MODEL_NUMBER_LOW));
gControlTable [CONTROL_MODEL_NUMBER_HIGH] = eeprom_read_byte ((uint8_t*)(EEPROM_BASE_ADDRESS + CONTROL_MODEL_NUMBER_HIGH));
gControlTable [CONTROL_FIRMWARE_VERSION] = eeprom_read_byte ((uint8_t*)(EEPROM_BASE_ADDRESS + CONTROL_FIRMWARE_VERSION));
gControlTable [CONTROL_ID] = eeprom_read_byte ((uint8_t*)(EEPROM_BASE_ADDRESS + CONTROL_ID));
gControlTable [CONTROL_BAUD_RATE] = eeprom_read_byte ((uint8_t*)(EEPROM_BASE_ADDRESS + CONTROL_BAUD_RATE));
gControlTable [CONTROL_RETURN_DELAY_TIME] = eeprom_read_byte ((uint8_t*)(EEPROM_BASE_ADDRESS + CONTROL_RETURN_DELAY_TIME));
gControlTable [CONTROL_STATUS_RETURN_LEVEL] = eeprom_read_byte ((uint8_t*)(EEPROM_BASE_ADDRESS + CONTROL_STATUS_RETURN_LEVEL));
}
else
UpdateEEPROM ();
 
// We copy the ID into its own global since we it them a lot...
gMyId = gControlTable [CONTROL_ID];
 
// We need to handle baud rate specifically, because it has to be set when it changes
if (gControlTable [CONTROL_BAUD_RATE] != 1)
HandleBaudRateChange (gControlTable [CONTROL_BAUD_RATE]);
}
 
 
//***************************************************************************
/**
* Check if given address & value are in range for writing...
*/
 
static uint8_t IsAddressLegalToWrite (uint8_t address, uint8_t value)
{
// Handle past end of writable area
if (address > CONTROL_DESIRED_WRIST_SPEED_HIGH)
return (FALSE);
 
if ((address >= CONTROL_FINGER_POSITION_LOW) && (address <= CONTROL_FINGER_PRESSURE_HIGH))
return (FALSE);
 
// Handle read-only areas...
if (address <= CONTROL_FIRMWARE_VERSION)
return (FALSE);
 
// Handle reserved areas...
if ((address >= 6) && (address <= 15))
return (FALSE);
if ((address >= 17) && (address <= 24))
return (FALSE);
 
// Special case to prevent messing up the ID
if ((address == CONTROL_ID) && ((value > 0xFD) || (value == 0)))
return (FALSE);
 
return (TRUE);
}
 
 
//***************************************************************************
/**
* Check if given address is in range for reading...
*/
 
static uint8_t IsAddressLegalToRead (uint8_t address)
{
// Handle past end of readable area
if (address > MAX_CONTROL_ENTRY)
return (FALSE);
 
return (TRUE);
}
 
 
//***************************************************************************
/**
* Read a byte from the Control Table
*/
 
static uint8_t ReadControlTableByte (uint8_t address)
{
// Log ("READ - [%2d] = %3d\n", address, gControlTable [address]);
// this function doesn't do much, but it is here in case we need to do something
// fancy in the future...
return (gControlTable [address]);
}
 
 
//***************************************************************************
/**
* Write a byte to the Control Table
*/
 
static void WriteControlTableByte (uint8_t address, uint8_t value)
{
// Log ("WRITE - [%2d] = %3d\n", address, value);
 
// first, update the control table in RAM
gControlTable [address] = value;
 
// if the address is an EEPROM address, write the byte into EEPROM
if (address <= MAX_EEPROM_ENTRY)
eeprom_write_byte ((uint8_t*)(EEPROM_BASE_ADDRESS + address), value);
 
// if the user updated the ID, then we need to reset our id cache
if (address == CONTROL_ID)
gMyId = value;
 
// finally, if the baud rate is updated, we need to update the UART
if (address == CONTROL_BAUD_RATE)
HandleBaudRateChange (value);
 
// TEMPORARY...If the user is updating one of the PID constants, update the variable.
/* if( address == CONTROL_WRIST_KP_HIGH )
wPk = (double)((value * 256) + gControlTable[ CONTROL_WRIST_KP_LOW ]) / 1000.0;
if( address == CONTROL_WRIST_KD_HIGH )
wDk = (double)((value * 256) + gControlTable[ CONTROL_WRIST_KD_LOW ]) / 1000.0;
if( address == CONTROL_WRIST_KI_HIGH )
wIk = (double)((value * 256) + gControlTable[ CONTROL_WRIST_KI_LOW ]) / 1000.0; */
// if( address == CONTROL_DESIRED_FINGER_SPEED_HIGH )
// speedOffset = (value * 256) + gControlTable[ CONTROL_DESIRED_FINGER_SPEED_LOW ];
}
 
 
//***************************************************************************
/**
* Called when we want to dump a response packet back...
*/
 
static void OutputResponse (uint8_t errorCode, uint8_t forceResponse)
{
if (errorCode != BLD_ERROR_NONE)
Log ("ERROR Response: 0x%02x\n", errorCode);
 
// we don't call this routine for READ_DATA, so we don't send a response unless
// the status return level is set to ALL... However, if forceResponse is true, then
// we're going to return a response anyways, because that is how PING is supposed to work
if (!forceResponse && (gControlTable [CONTROL_STATUS_RETURN_LEVEL] != BLD_STATUS_RETURN_LEVEL_ALL))
return;
 
us_spin (gControlTable [CONTROL_RETURN_DELAY_TIME] * 2);
UART0_PutChar (0xFF);
UART0_PutChar (0xFF);
UART0_PutChar (gMyId);
UART0_PutChar (0x02); // length for blank response is always 2
UART0_PutChar (errorCode);
UART0_PutChar (~(gMyId + 0x02 + errorCode)); // checksum is ~(ID + LENGTH + ERROR)
}
 
 
//***************************************************************************
/**
* Called for a READ command
*/
 
static void HandleReadCommand (BLD_Packet_t *packet, uint8_t isBroadcast)
{
uint8_t length, startAddress;
uint8_t crc, index, byte;
 
// Doing a READ_DATA via the broadcast ID is a no-op...
if (isBroadcast)
return;
 
startAddress = packet->m_param [0];
length = packet->m_param [1];
 
// We do the loop once to check, because reading should be atomic (it either works or doesn't)
for (index = startAddress; index < (startAddress + length); index++)
{
if (!IsAddressLegalToRead (index))
{
if (!isBroadcast)
OutputResponse (BLD_ERROR_RANGE, FALSE);
return;
}
}
 
// Reponse packet looks like this:
// HEADER HEADER ID LENGTH ERROR PARAM(s) CHECKSUM
 
us_spin (gControlTable [CONTROL_RETURN_DELAY_TIME] * 2);
crc = gMyId + length + 2;
UART0_PutChar (0xFF);
UART0_PutChar (0xFF);
UART0_PutChar (gMyId);
UART0_PutChar (length + 2); // length for READ
UART0_PutChar (0x00); // error is 0, since we got here...
for (index = startAddress; index < (startAddress + length); index++)
{
byte = ReadControlTableByte (index);
UART0_PutChar (byte);
crc += byte;
}
UART0_PutChar (~crc); // checksum is ~(ID + LENGTH + ERROR + PARAMs)
}
 
 
//***************************************************************************
/**
* Called for a WRITE command
*/
 
// HEADER HEADER ID LENGTH ERROR PARAM(s) CHECKSUM
 
static void HandleWriteCommand (BLD_Packet_t *packet, uint8_t isBroadcast)
{
uint8_t length, startAddress;
uint8_t index;
 
startAddress = packet->m_param [0];
length = packet->m_length - 3;
 
// We do the loop once to check, because writing should be atomic (it either works or doesn't)
for (index = 1; index <= length; index++)
{
if (!IsAddressLegalToWrite (startAddress + index - 1, packet->m_param [index]))
{
if (!isBroadcast)
OutputResponse (BLD_ERROR_RANGE, FALSE);
return;
}
}
 
// Now, we know we're legal, so do the actual write...
for (index = 1; index <= length; index++)
WriteControlTableByte (startAddress + index - 1, packet->m_param [index]);
 
// we don't send a response packet back for a broadcast WRITE_DATA
if (!isBroadcast)
OutputResponse (BLD_ERROR_NONE, FALSE);
}
 
 
//***************************************************************************
/**
* Called when a packet is parsed.
*/
 
static void PacketReceived (BLD_Instance_t *inst, BLD_Packet_t *packet, BLD_Error_t err)
{
 
if (err == BLD_ERROR_NONE)
{
// if we get here, we know we have a good packet...
uint8_t isBroadcast = packet->m_id == BLD_BROADCAST_ID;
 
// we really only care about packets for us...
if ((packet->m_id == gMyId) || isBroadcast)
{
switch (packet->m_cmd)
{
case BLD_CMD_PING:
{
// Log ("got PING command\n");
OutputResponse (BLD_ERROR_NONE, TRUE);
break;
}
 
case BLD_CMD_READ:
{
// Log ("got READ command\n");
HandleReadCommand (packet, isBroadcast);
break;
}
 
case BLD_CMD_WRITE:
{
// Log ("got WRITE command\n");
HandleWriteCommand (packet, isBroadcast);
break;
}
 
case BLD_CMD_RESET:
{
Log ("got RESET command\n");
InitializeControlTable (TRUE);
OutputResponse (BLD_ERROR_NONE, FALSE);
break;
}
 
default:
{
// there are other commands that we don't care about....
Log ("ID:0x%02x Cmd: 0x%02x *** Unknown ***\n", packet->m_id, packet->m_cmd);
break;
}
}
}
// else
// Log ("Got packet for ID: %3d\n", packet->m_id);
}
else if (packet->m_id == gMyId)
{
Log ("CRC Error\n");
OutputResponse (BLD_ERROR_CHECKSUM, FALSE);
}
 
} // PacketReceived
 
 
//***************************************************************************
/**
* Update the control table from the sensor values
*/
 
static void UpdateSensorValues (void)
{
uint16_t value;
 
disableMotor();
ms_spin (10);
 
value = ADC_Read (GRIPPER_PRESSURE_CHANNEL);
gControlTable [CONTROL_FINGER_PRESSURE_LOW] = (value & 0xFF);
gControlTable [CONTROL_FINGER_PRESSURE_HIGH] = (value >> 8);
 
value = ADC_Read (GRIPPER_POSITION_CHANNEL);
gControlTable [CONTROL_FINGER_POSITION_LOW] = (value & 0xFF);
gControlTable [CONTROL_FINGER_POSITION_HIGH] = (value >> 8);
 
value = ADC_Read (GRIPPER_ROTATION_CHANNEL);
gControlTable [CONTROL_WRIST_POSITION_LOW] = (value & 0xFF);
gControlTable [CONTROL_WRIST_POSITION_HIGH] = (value >> 8);
 
enableMotor();
}
 
static int16_t max (int16_t firstValue, int16_t secondValue)
{
if (firstValue > secondValue)
return (firstValue);
else
return (secondValue);
}
 
static int16_t min (int16_t firstValue, int16_t secondValue)
{
if (firstValue < secondValue)
return (firstValue);
else
return (secondValue);
}
 
 
//***************************************************************************
/**
* Update the motors from the control table
*/
 
static void UpdateActuators (void)
{
int16_t desiredFingerPosition, desiredWristPosition;
int16_t desiredFingerSpeed, desiredWristSpeed;
int16_t fingerPosition, wristPosition;
int16_t offset;
int16_t offsetDelta;
int16_t fingerMotorSpeed, wristMotorSpeed;
int16_t rawFingerMotorSpeed, rawWristMotorSpeed;
double scale;
 
// Get the finger position and speed requested by the controller.
desiredFingerPosition = gControlTable [CONTROL_DESIRED_FINGER_POSITION_LOW] +
(gControlTable [CONTROL_DESIRED_FINGER_POSITION_HIGH] * 256);
desiredFingerSpeed = gControlTable [CONTROL_DESIRED_FINGER_SPEED_LOW] +
(gControlTable [CONTROL_DESIRED_FINGER_SPEED_HIGH] * 256);
 
// Get the actual current finger position.
fingerPosition = gControlTable [CONTROL_FINGER_POSITION_LOW] +
(gControlTable [CONTROL_FINGER_POSITION_HIGH] * 256);
 
// Now determine how far off we are.
offset = fingerPosition - desiredFingerPosition;
 
// Determine how fast we are appropaching the desired position
// (ie. what is the change in offset from last time to this time).
offsetDelta = offset - lastFingerOffset;
lastFingerOffset = offset;
 
// Determine how much overall error we have had (ie. keep a running
// sum of our offsets).
fingerErrorAccumulation += offset;
 
// Use PID to determine the correct speed at which to drive the motor.
// The further away the actual position is from the desired position,
// the greater the speed. We need to bracket this so the value does
// not exceed our possible speed values (-1023 to 1023). If the offset
// is small enough, we just accept it's current position.
if( abs( offset ) > FINGER_POSITION_DEADBAND ) {
fingerMotorSpeed = (int16_t)(fPk * (double)offset + fDk * (double)offsetDelta + fIk * (double)fingerErrorAccumulation);
fingerMotorSpeed = min( max( fingerMotorSpeed, -1023 ), 1023 );
}
else
fingerMotorSpeed = 0;
 
// Get the wrist position and speed requested by the controller.
desiredWristPosition = gControlTable [CONTROL_DESIRED_WRIST_POSITION_LOW] +
(gControlTable [CONTROL_DESIRED_WRIST_POSITION_HIGH] * 256);
desiredWristSpeed = gControlTable [CONTROL_DESIRED_WRIST_SPEED_LOW] +
(gControlTable [CONTROL_DESIRED_WRIST_SPEED_HIGH] * 256);
 
// Get the actual current wrist position.
wristPosition = gControlTable [CONTROL_WRIST_POSITION_LOW] +
(gControlTable [CONTROL_WRIST_POSITION_HIGH] * 256);
 
// Now determine how far off we are.
offset = wristPosition - desiredWristPosition;
 
// Determine how fast we are appropaching the desired position
// (ie. what is the change in offset from last time to this time).
offsetDelta = offset - lastWristOffset;
lastWristOffset = offset;
 
// Determine how much overall error we have had (ie. keep a running
// sum of our offsets).
wristErrorAccumulation += offset;
 
// Use PID to determine the correct speed at which to drive the motor.
// The further away the actual position is from the desired position,
// the greater the speed. We need to bracket this so the value does
// not exceed our possible speed values (-1023 to 1023). If the offset
// is small enough, we just accept it's current position.
if( abs( offset ) > WRIST_POSITION_DEADBAND ) {
wristMotorSpeed = wPk * (double)offset + wDk * (double)offsetDelta + wIk * (double)wristErrorAccumulation;
wristMotorSpeed = min( max( wristMotorSpeed, -1023 ), 1023 );
}
else
wristMotorSpeed = 0.0;
 
// fingerMotorSpeed += ((speedOffset * 2) - 1023);
// if( abs( fingerMotorSpeed ) < 2 )
// fingerMotorSpeed = 0;
 
// We are using 8-bit anti-phase PWM, so we need a number from 0 to 255, where 127 is off.
// To get this we need to scale our motor speed (-1023, 1023) accordingly. We also must
// accomodate the motor deadband (measured symmetrically about zero).
// The usable motor speed range is (127 - deadband) either side of STOP.
scale = (double)(127 - FINGER_MOTOR_DEADBAND) / 1023.0;
rawFingerMotorSpeed = (int16_t)((double)fingerMotorSpeed * scale);
 
scale = (double)(127 - WRIST_MOTOR_DEADBAND) / 1023.0;
rawWristMotorSpeed = (int16_t)((double)wristMotorSpeed * scale);
 
// Now we shift the motorSpeed by the deadband amount.
if( fingerMotorSpeed != 0 ) {
if( fingerMotorSpeed > 0 )
rawFingerMotorSpeed += FINGER_MOTOR_DEADBAND;
else
rawFingerMotorSpeed -= FINGER_MOTOR_DEADBAND;
}
 
if( wristMotorSpeed != 0 ) {
if( wristMotorSpeed > 0 )
rawWristMotorSpeed += WRIST_MOTOR_DEADBAND;
else
rawWristMotorSpeed -= WRIST_MOTOR_DEADBAND;
}
 
// Now we shift the (-127, 127) speed up to (0, 254) by adding 127.
rawFingerMotorSpeed += 127;
rawWristMotorSpeed += 127;
 
if (gCycleCount == 0)
{
// Log ( "FP: %4d DFP: %4d DP: %4d FMS: %4d RFMS: %4d kD: %4d\n", fingerPosition, desiredFingerPosition, offset, fingerMotorSpeed, rawFingerMotorSpeed, (int16_t)(fDk * 1000.0) );
// Log ( "WP: %4d DWP: %4d DP: %4d WMS: %4d RWMS: %4d\n", wristPosition, desiredWristPosition, offset, wristMotorSpeed, rawWristMotorSpeed );
}
 
// SetMotorSpeed( SPEED_OFF, rawFingerMotorSpeed );
SetMotorSpeed( rawWristMotorSpeed, rawFingerMotorSpeed );
}
 
//***************************************************************************
/**
* Update the LED from the control table
*/
 
static void UpdateLED (void)
{
if (gControlTable [CONTROL_LED])
LED_PORT &= ~LED_MASK;
else
LED_PORT |= LED_MASK;
}
 
 
//***************************************************************************
/**
* Bioloid Gripper
*/
 
int main (void)
{
BLD_Instance_t inst;
tick_t previousCount;
 
// initialize the hardware stuff we use
InitTimer ();
BLD_InitUART ();
InitTimerUART ();
ADC_Init (ADC_PRESCALAR_AUTO);
InitMotors ();
 
// initialize the packet parser
BLD_Init (&inst);
inst.m_pktRcvd = PacketReceived;
 
// initialize the control table from EEPROM
InitializeControlTable (FALSE);
 
// set the LED port for output
LED_DDR |= LED_MASK;
 
// Flash the LED for 1/2 a second...
LED_PORT &= ~LED_MASK;
ms_spin (500);
LED_PORT |= LED_MASK;
 
Log ("*****\n");
Log ("***** Bioloid Gripper\n");
Log ("***** Copyright 2007 HUVrobotics\n");
Log ("*****\n");
 
gCycleCount = 0;
while (1) // outer loop is once every 10 ms
{
previousCount = gTickCount;
while (gTickCount == previousCount)
{
if (UART0_IsCharAvailable ())
{
uint8_t ch = UART0_GetChar ();
// Log ("Got: %3d\n", ch);
BLD_ProcessChar (&inst, ch);
}
}
// every 10 ms, we want to update the control table from the IMU,
// and update the LED and actuators from the control table.
// UpdateSensorValues ();
if( sensorCycleCount == 0 )
UpdateSensorValues ();
 
UpdateActuators ();
UpdateLED ();
gCycleCount++;
if (gCycleCount == 100)
gCycleCount = 0;
 
sensorCycleCount++;
if (sensorCycleCount == 10)
sensorCycleCount = 0;
}
 
// we'll never get here...
return 0;
}
 
/avr/bioloid-gripper/Makefile
0,0 → 1,19
###########################################################################
#
# bioloidGripper Makefile
#
###########################################################################
 
MK_OS = avr
MK_AVR_MCU = m168
MK_AVR_FREQ = 8MHz
 
MK_AVR_PROGRAMMER ?= stk500
 
# Extended fuse byte should be FB for standalone, FA for bootloader-stk500
MK_AVR_FUSES_m168 = FB DF FF
 
MK_HEX_TARGET = bioloidGripper
MK_SRC_FILES = bioloidGripper.c bioloid-pkt.c Motor.c Delay.c adc.c Log.c StrPrintf.o Timer-UART.c bioloid-uart.c Timer.c
 
include ../../rules/mkRules.mk