neocturne/rc2007-soccer
Archived
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Added lot's of code-files used during work

Browse source
This commit is contained in:
sicarius 2007-02-11 18:32:03 +00:00
parent 644121b478
commit 56d9bdd39e
155 changed files with 23423 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

96
source/qFix/qfix.h Normal file
View file

@ -0,0 +1,96 @@
//------------------------------------------------------------------
// qfix.h
//
// This file contains general includes and definitions for the
// family of qfix boards.
//
// Copyright 2004-2006 by KTB mechatronics GmbH
// Author: Stefan Enderle
//------------------------------------------------------------------
#ifndef qfix_h
#define qfix_h
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
/* defines for compatibility */
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif
#ifndef BV
#define BV _BV
#endif
/** Returns the absolute value of a
*/
inline
int abs(int a)
{
if (a>=0) return a;
else return -a;
}
/** Sleeps for sec seconds. (Note: this is tuned for
* an Atmel ATmega32 running with 8 MHz.)
*/
inline
void sleep(int sec)
{
for (int s=0; s<sec; s++) {
for (long int i=0; i<702839; i++) {
asm volatile("nop");
}
}
}
/** Sleeps for msec milliseconds. (Note: this is tuned for
* an Atmel ATmega32 running with 8 MHz.)
*/
inline
void msleep(int msec)
{
for (int s=0; s<msec; s++) {
for (long int i=0; i<703; i++) {
asm volatile("nop");
}
}
}
/** Translates the given number into a string which ends
* with 0.
*/
inline
void num2str(int num, char* str)
{
bool showZero = false;
int i=0;
int n=abs(num);
if (num<0) str[i++]='-';
if (n>=10000 || showZero) { str[i++]=(n/10000)+'0'; n=n % 10000; showZero=true; }
if (n>=1000 || showZero) { str[i++]=(n/1000) +'0'; n=n % 1000; showZero=true; }
if (n>=100 || showZero) { str[i++]=(n/100) +'0'; n=n % 100; showZero=true; }
if (n>=10 || showZero) { str[i++]=(n/10) +'0'; n=n % 10; showZero=true; }
str[i++]=n +'0'; // last digit
str[i++]=0; // ending 0
}
#endif

344
source/qFix/qfixBobbyBoard.h Normal file
View file

@ -0,0 +1,344 @@
//------------------------------------------------------------------
// qfixBobbyBoard.h
//
// This file contains the class BobbyBoard which represents the
// physical BobbyBoard with all its inputs and outputs.
//
// Copyright 2004-2006 by KTB mechatronics GmbH
// Author: Stefan Enderle
//------------------------------------------------------------------
#include "qfix.h"
#ifndef qfixBobbyBoard_h
#define qfixBobbyBoard_h
static int speedMotor0 = 0;
static int speedMotor1 = 0;
/**
* \class BobbyBoard
* \brief Represents the controller board "BobbyBoard".
* \author Stefan Enderle
*
* The class BobbyBoard represents the
* physical BobbyBoard with all its inputs and outputs.
* With this class it is possible to drive the motors,
* put on LEDs, check the buttons and get data from the
* analog and digital inputs.
*/
class BobbyBoard
{
public:
/** Constructor for the BobbyBoard class. All LEDs and motors are put off,
* the power outputs are put on.
*/
BobbyBoard();
/** Puts on LED i. i must be 0..3
* \see ledOff
*/
void ledOn(int i);
/** Puts off LED i. i must be 0..3
* \see ledOn
*/
void ledOff(int i);
/** Puts off all LEDs.
*/
void ledsOff();
/** Puts LED i on if state is true, else off. i must be 0..3
* \see ledOn, ledOff
*/
void led(int i, bool state);
/** Puts the power output i on. i must be 0..3
* \see powerOff
*/
void powerOn(int i);
/** Puts the power output i off. i must be 0..3
* \see powerOn
*/
void powerOff(int i);
/** Puts the power output i on if state is true, else off. i must be 0..3
* \see powerOn, powerOff
*/
void power(int i, bool state);
/** Checks the state of button i. If it is pressed, true is returned,
* else false. i must be 0..3
* \see waitForButton
*/
bool button(int i);
/** Uses the four LEDs on the board to display the value i
* with 0 <= i <= 255
*/
void ledMeter(int i);
/** Sets motor i to the given speed. -255 <= speed <= 255
* \see motors
*/
void motor(int i, int speed);
/** Sets both motors to the given speed. -255 <= speed <= 255
* \see motor
*/
void motors(int speed0, int speed1);
/** Puts off both motors.
* \see motor, motors
*/
void motorsOff();
/** returns the value of the analog port i. i must be 0..3
* \see digital
*/
int analog(int i);
/** returns true if the digital port is logical high, else false. i must be 0..3
* \see analog
*/
bool digital(int i);
/** Waits until button i is pressed and released again. i must be 0..3
* \see button
*/
void waitForButton(int i);
/** Waits for i seconds.
*/
void sleep(int i);
/** Waits for i milliseconds.
*/
void msleep(int i);
};
// PWM routine //
SIGNAL (SIG_OVERFLOW0)
{
const int OFFSET=50; // motor does not work with very low ratio
static int counter=255+OFFSET;
if (speedMotor0==0) cbi(PORTC, 2); // enable1 = 0
else if (abs(speedMotor0)+OFFSET >= counter) sbi(PORTC, 2); // enable1 = 1
else cbi(PORTC, 2); // enable1 = 0
if (speedMotor1==0) cbi(PORTC, 3); // enable1 = 0
else if (abs(speedMotor1)+OFFSET >= counter) sbi(PORTC, 3); // enable2 = 1
else cbi(PORTC, 3); // enable2 = 0
if (counter==0) counter=255+OFFSET;
else counter--;
}
void initTimer()
{
TCCR0=1; // timer 0 for interrupt
TIMSK=1;
sei(); // enable interrupts
}
BobbyBoard::BobbyBoard()
{
// PORT A: Analog In + buttons //
DDRA=0; // all bits input
PORTA=16+32+64+128; // set pullups for buttons
ADCSRA=128; // set A/D enable bit (ADEN)
// PORT B: LEDs + digital Input //
DDRB = 16+32+64+128; // upper bits output: LEDs
PORTB |= 16+32+64+128; // set bits 5 to 8 -> LEDs off
PORTB |= 1+2+4+8; // set pullups for digital inputs
// PORT C: I2C and motors //
DDRC = 255-1-2; // direction port C, all bits output except I2C
PORTC = 1+2; // clear bits 2-7 -> motors off; set bits 0,1 -> I2C pullUps
// PORT D: Power Output //
DDRD = 255; // direction port D, all bits output
PORTD = 0; // clear all bits -> power on
initTimer();
}
void BobbyBoard::ledOn(int i)
{
if ((i<0) || (i>3)) return;
cbi(PORTB, i+4); // clear bit -> LED on
}
void BobbyBoard::ledOff(int i)
{
if ((i<0) || (i>3)) return;
sbi(PORTB, i+4); // set bit -> LED off
}
void BobbyBoard::ledsOff()
{
PORTB|=128+64+32+16; // set high bits -> LEDs off
}
void BobbyBoard::led(int i, bool state)
{
if (state) ledOn(i); else ledOff(i);
}
void BobbyBoard::powerOn(int i)
{
if ((i<0) || (i>7)) return;
cbi(PORTD, i);
}
void BobbyBoard::powerOff(int i)
{
if ((i<0) || (i>7)) return;
sbi(PORTD, i);
}
void BobbyBoard::power(int i, bool state)
{
if (state) powerOn(i); else powerOff(i);
}
bool BobbyBoard::button(int i)
{
if ((i<0) || (i>3)) return false;
return ( (PINA & (16<<i)) == 0);
}
void BobbyBoard::ledMeter(int i)
{
led(0, (i>50));
led(1, (i>100));
led(2, (i>150));
led(3, (i>200));
}
void BobbyBoard::motor(int i, int speed)
{
if ((i<0) || (i>1)) return;
if (i==0) {
speedMotor0 = speed;
if (speed==0) {
cbi(PORTC, 4); // input1 = 0
cbi(PORTC, 5); // input2 = 0
}
else if (speed>0) {
sbi(PORTC, 4); // input1 = 1
cbi(PORTC, 5); // input2 = 0
}
else {
cbi(PORTC, 4); // input1 = 0
sbi(PORTC, 5); // input2 = 1
}
}
if (i==1) {
speedMotor1 = speed;
if (speed==0) {
cbi(PORTC, 6); // input3 = 0
cbi(PORTC, 7); // input4 = 0
}
else if (speed>0){
sbi(PORTC, 6); // input3 = 1
cbi(PORTC, 7); // input4 = 0
}
else {
cbi(PORTC, 6); // input3 = 0
sbi(PORTC, 7); // input4 = 1
}
}
}
void BobbyBoard::motors(int motor0, int motor1)
{
motor(0,motor0);
motor(1,motor1);
}
void BobbyBoard::motorsOff()
{
motor(0,0);
motor(1,0);
}
// return 0-255 //
int BobbyBoard::analog(int i)
{
if ((i<0) || (i>3)) return -1;
ADMUX=i; // select analog input and start A/D
sbi(ADMUX, ADLAR); // left adjust -> we use only ADCH
sbi(ADCSRA, ADSC); // start conversion
while (ADCSRA & 64); // wait until ADSC is low again
int value = ADCH; // read 8 bit value fom ADCH
return value;
}
bool BobbyBoard::digital(int i)
{
if ((i<0) || (i>3)) return false; // bad solution...
return (PINB & (1<<i)) ;
}
void BobbyBoard::waitForButton(int i)
{
if ((i<0) || (i>3)) return; // bad solution...
while (!button(i)) { /* do nothing */ }
while (button(i)) { /* do nothing */ }
}
void BobbyBoard::sleep(int i)
{
::sleep(i);
}
void BobbyBoard::msleep(int i)
{
::msleep(i);
}
#endif

322
source/qFix/qfixCAN128Board.h Normal file
View file

@ -0,0 +1,322 @@
//------------------------------------------------------------------
// qfixCAN128Board.h
//
// This file contains the class CAN128Board which represents the
// physical Can128Board with all its inputs and outputs.
//
// Copyright 2005-2006 by KTB mechatronics GmbH
// Author: Stefan Enderle, Florian Schrapp
//------------------------------------------------------------------
#ifndef qfixCAN128Board_h
#define qfixCAN128Board_h
#include "qfix.h"
static int speedMotor0 = 50;
static int speedMotor1 = 50;
class CAN128Board
{
public:
/** Constructor for the robot board class.
*/
CAN128Board();
/** Puts on LED i
*/
void ledOn(int i);
/** Puts off LED i
*/
void ledOff(int i);
/** Puts off all LEDs
*/
void ledsOff();
/** Puts LED i on if state is true, else off
*/
void led(int i, bool state);
/** Uses the four LEDs on the board to display the value i
* with 0 <= i <= 255
*/
void ledMeter(int i);
/** Puts the power output i on
*/
void powerOn(int i);
/** Puts the power output i off
*/
void powerOff(int i);
/** Puts the power output i on if state is true, else off
*/
void power(int i, bool state);
/** Checks the state of button i. If it is pressed, true is returned,
* else false.
*/
bool button(int i);
/** Sets motor i to the given speed. -255 <= speed <= 255
*/
void motor(int i, int speed);
/** Sets both motors to the given speed. -255 <= speed <= 255
*/
void motors(int speed0, int speed1);
/** Puts off both motors.
*/
void motorsOff();
/** returns the value of the analog port i. 0 <= value <= 255
*/
int analog(int i);
/** returns true if the digital port is logical high, else false.
*/
bool digital(int i);
};
// PWM routine //
SIGNAL (SIG_OVERFLOW0)
{
const int OFFSET=50; // motor does not work with very low ratio
static int counter=255+OFFSET;
if (speedMotor1==0) cbi(PORTB, 6); // enable1 = 0
else if (abs(speedMotor1)+OFFSET >= counter) sbi(PORTB, 6); // enable1 = 1
else cbi(PORTB, 6); // enable1 = 0
if (speedMotor0==0) cbi(PORTB, 7); // enable2 = 0
else if (abs(speedMotor0)+OFFSET >= counter) sbi(PORTB, 7); // enable2 = 1
else cbi(PORTB, 7); // enable2 = 0
if (counter==0) counter=255+OFFSET;
else counter--;
}
void initTimer()
{
TCCR0A=1; // timer 0 for interrupt
TIMSK1=1;
sei(); // enable interrupts
}
CAN128Board::CAN128Board()
{
// Port A: motors output
DDRA = 4+8+16+64; // all motors output
PORTA = 0; // clear all bits = motors off
// Port B: motors enable
DDRB = 64+128; // upper bits output
PORTB = 0; // clear bit 6 and 7 = motors off
// Port C: power output
DDRC = 255; // default direction port C: all bits output
PORTC = 0; // clear all bits: power on
// Port D: I2C //
DDRD = 0; // all bits input
PORTD = 1+2; // set bits 0,1 -> I2C pullUps
// Port E: leds + buttons
DDRE = 4+8+16+32; // pin 2-5 leds output; pin 6+7 input for buttons 0+1
PORTE |= 4+8+16+32; // set leds off
PORTE |= 64+128; // set pullups for buttons 0+1
// Port F: analog in + digital in
DDRF = 0; // all bits input
// Port G: buttons
DDRG = 0; // pin 3+4 input for buttons 2+3
PORTG |= 8+16; // set pullups for buttons 3+4
initTimer();
}
void CAN128Board::ledOn(int i)
{
if ((i<0) || (i>3)) return;
cbi(PORTE, i+2); // clear bit -> LED on
}
void CAN128Board::ledOff(int i)
{
if ((i<0) || (i>3)) return;
sbi(PORTE, i+2); // set bit -> LED off
}
void CAN128Board::ledsOff()
{
ledOff(0);
ledOff(1);
ledOff(2);
ledOff(3);
}
void CAN128Board::led(int i, bool state)
{
if (state) ledOn(i); else ledOff(i);
}
void CAN128Board::ledMeter(int i)
{
led(0, (i>50));
led(1, (i>100));
led(2, (i>150));
led(3, (i>200));
}
void CAN128Board::powerOn(int i)
{
if ((i<0) || (i>7)) return;
cbi(PORTC, i);
}
void CAN128Board::powerOff(int i)
{
if ((i<0) || (i>7)) return;
sbi(PORTC, i);
}
void CAN128Board::power(int i, bool state)
{
if (state) powerOn(i); else powerOff(i);
}
bool CAN128Board::button(int i)
{
if ((i<0) || (i>3)) return false;
else if (i==0 || i ==1){
return ((PINE & (64<<i)) == 0); //wenn pina=0 button gedrückt => 0&1=false, also 0==0 => true
}
else if (i==2 || i==3){
return ((PING & (2<<i)) == 0);
}
else return false;
}
void CAN128Board::motor(int i, int speed)
{
if ((i<0) || (i>1)) return;
if (i==1) {
speedMotor1 = speed;
if (speed==0) {
cbi(PORTA, 2); // input1 = 0
cbi(PORTA, 3); // input2 = 0
//cbi(PORTB, 6); // enable1 = 0
}
else if (speed>0) {
sbi(PORTA, 2); // input1 = 1
cbi(PORTA, 3); // input2 = 0
//sbi(PORTB, 6); // enable1 = 1
}
else {
cbi(PORTA, 2); // input1 = 0
sbi(PORTA, 3); // input2 = 1
}
}
if (i==0) {
speedMotor0 = speed;
if (speed==0) {
cbi(PORTA, 4); // input3 = 0
cbi(PORTA, 5); // input4 = 0
//cbi(PORTB, 7); // enable2 = 0
}
else if (speed>0){
sbi(PORTA, 4); // input3 = 1
cbi(PORTA, 5); // input4 = 0
//sbi(PORTB, 7); // enable2 = 1
}
else {
cbi(PORTA, 4); // input3 = 0
sbi(PORTA, 5); // input4 = 1
}
}
}
void CAN128Board::motors(int motor0, int motor1)
{
motor(0,motor0);
motor(1,motor1);
}
void CAN128Board::motorsOff()
{
motor(0,0);
motor(1,0);
}
// return 0-255 //
int CAN128Board::analog(int i)
{
if ((i<0) || (i>7)) return -1;
ADCSRA=128; // set A/D enable bit (ADEN)
ADMUX=i; // select analog input and start A/D
sbi(ADMUX, ADLAR); // left adjust -> we use only ADCH
sbi(ADCSRA, ADSC); // start conversion
while (ADCSRA & 64); // wait until ADSC is low again
int value = ADCH; // read 8 bit value fom ADCH
return value;
}
bool CAN128Board::digital(int i)
{
if ((i<0) || (i>7)) return false; // bad solution...
ADCSRA=0; // clear A/D enable bit (ADEN)
PORTF |= (1<<i); // set pullups for digital inputs
return (PINF & (1<<i)) ;
}
#endif

278
source/qFix/qfixI2C.h Normal file
View file

@ -0,0 +1,278 @@
//------------------------------------------------------------------
// qfixI2C.h
//
// This class is used for low-level I2C communication.
//
// For TW_ constants see compat/twi.h
//
// Copyright 2005-2006 by KTB mechatronics GmbH
// Author: Stefan Enderle, Florian Schrapp
//------------------------------------------------------------------
#ifndef qfixI2C_h
#define qfixI2C_h
#include "qfix.h"
#include "compat/twi.h"
const int ACTION_SEND = 1;
const int ACTION_GET = 2;
const int ACTION_UNKNOWN = 3;
const uint8_t ERROR_NO_ACK = 1;
const uint8_t ERROR_NO_START = 2;
const uint8_t ERROR_NOT_SENT = 3; // send() could not send byte(s)
class I2C
{
private:
uint8_t err;
uint8_t adr;
void sendStartSLA_W(uint8_t address);
void sendStartSLA_R(uint8_t address);
void sendByte(uint8_t data);
void sendStop();
void readByte(uint8_t& data);
public:
I2C();
uint8_t error();
// master side //
void send(uint8_t address, uint8_t data);
void send(uint8_t address, uint8_t* data, int length);
void get(uint8_t address, uint8_t* data, int length);
// slave side //
void setSlaveAdress(uint8_t address);
bool isAction();
bool isActionSend(); // true if master sent something
bool isActionGet(); // true if master wants to get something
int receive(uint8_t* data); // if action is send, receive the bytes
void returnBytes(uint8_t* data, int len, bool lastOne);
};
I2C::I2C()
{
TWBR = 10; // I2C bitrate (must be 10 or higher)
TWCR = 4+64; // TWI enable bit + ackn
TWSR = 0; // prescaler
// I2C pull-ups are set in the board file //
err=0;
}
uint8_t I2C::error()
{
return err;
}
void I2C::setSlaveAdress(uint8_t address)
{
TWAR = address<<1; // slave address (shift by one since bit 0 has different meaning)
}
bool I2C::isAction()
{
if (TWCR&(1<<TWINT)) return true;
else return false;
}
inline
bool I2C::isActionGet()
{
return ((TWSR&248)==TW_ST_SLA_ACK); // own I2C address received, read mode, ACK sent
}
inline
bool I2C::isActionSend()
{
return ((TWSR&248)==TW_SR_SLA_ACK); // own I2C address received, write mode, ACK sent
}
void I2C::sendStartSLA_W(uint8_t address)
{
bool OK = false;
int counter = 0;
do {
TWCR |= BV(TWINT)|BV(TWSTA); // send start condition
while(!(TWCR&BV(TWINT))); // wait for OK
if ((TWSR&248)!=TW_START) { // start condition was sent -> OK
err=ERROR_NO_START;
return;
}
TWDR=(address<<1); // slave address + write mode
TWCR = BV(TWINT) | BV(TWEN) | BV(TWEA); // generate command
while(!(TWCR&128)) {}; // wait for OK
if ((TWSR&248)==TW_MT_SLA_ACK) OK=true; // SLA+W was sent, ACK received -> OK
if ((TWSR&248)==TW_MT_SLA_NACK) { // SLA+W was sent, ACK not received -> ERROR
counter++;
if (counter == 10) { // After 10 tries...
err=ERROR_NO_ACK; // ... return with ERROR_NO_ACK
return;
}
}
} while(!OK);
err=0; // OK
}
void I2C::sendStartSLA_R(uint8_t address)
{
bool OK=false;
while (!OK) {
TWCR |= BV(TWINT)|BV(TWSTA); // send start condition
// sbi(TWCR, TWSTA); // generate start condition
while(!(TWCR&128)); // wait for OK
if ((TWSR&248)==8); // start condition was sent -> OK
TWDR=(address<<1) | 1; // slave address + read mode
TWCR = BV(TWINT) | BV(TWEN) | BV(TWEA); // generate command
while(!(TWCR&128)); // wait for send OK + ACK/NACK from slave
if ((TWSR&248)==0x38); // Arbitration lost or NOT ACK -> ERROR
if ((TWSR&248)==0x40) OK=true; // SLA+R was sent, ACK received -> OK
if ((TWSR&248)==0x48); // SLA+W was sent, ACK not received -> ERROR
sbi(TWCR, TWINT); // OK
}
}
// This is used by send() called by the master //
void I2C::sendByte(uint8_t data)
{
TWDR = data; // send one data byte
TWCR = (1<<TWINT)|(1<<TWEA)|(1<<TWEN); // start transmission
while(!(TWCR&(1<<TWINT))); // wait for TWCR to become 0
if ((TWSR&248)==TW_MT_DATA_ACK); // data sent, ACK received -> OK
if ((TWSR&248)==TW_MT_DATA_NACK); // data sent, ACK not received -> ERROR
}
void I2C::sendStop()
{
TWCR = BV(TWINT) | BV(TWEN) | BV(TWEA) | BV(TWSTO); // generate stop command
}
// Note: in qfix, the I2C address is the board identifier
// and the first data byte ist the logical ID
inline
void I2C::send(uint8_t address, uint8_t data)
{
send(address, &data, 1);
}
// Note: in qfix, the I2C address is the board identifier
// and the first data byte ist the logical ID
void I2C::send(uint8_t address, uint8_t* data, int length)
{
sendStartSLA_W(address);
if (err!=0)
{
sendStop();
err = ERROR_NOT_SENT;
return;
}
for (int i=0; i<length; i++) {
sendByte(data[i]);
}
sendStop();
err = 0; // OK
}
int I2C::receive(uint8_t* data)
{
TWCR = (1<<TWINT)|(1<<TWEA)|(1<<TWEN);
while(!(TWCR&(1<<TWINT))); // wait for something
int len = 0;
while ((TWSR&248)==TW_SR_DATA_ACK) { // data received (in TWDR), ACK sent
data[len] = TWDR; // read received byte
len++;
TWCR = (1<<TWINT)|(1<<TWEA)|(1<<TWEN);
while(!(TWCR&(1<<TWINT))) { } // wait for OK
}
if ((TWSR&248)==TW_SR_STOP) { } // STOP (or new START) received -> OK
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA); // generate command
return len;
}
void I2C::readByte(uint8_t& data)
{
while(!isAction());
if ((TWSR&248)==0x50); // data received: NACK
if ((TWSR&248)==0x58); // data received: ACK
data = TWDR;
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA); // generate command
}
void I2C::get(uint8_t address, uint8_t* data, int length)
{
sendStartSLA_R(address);
if (err!=0)
{
sendStop();
err = ERROR_NOT_SENT;
return;
}
for (int i=0; i<length; i++) {
readByte(data[i]);
}
sendStop();
}
void I2C::returnBytes(uint8_t* data, int len, bool lastOne)
{
for (int i=0; i<len; i++) {
TWDR=data[i]; // byte to send
if (i==len-1) TWCR = (1<<TWINT) | (1<<TWEN); // command for last byte (no TWEA is correct!)
else TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA); // command for all others
while(!(TWCR&(1<<TWINT))); // wait for OK
if ((TWSR&248)==0xB8); // data sent, ACK ACK received -> OK
if ((TWSR&248)==0xC0); // data sent, ACK not received -> ERROR
}
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA); // generate command
}
#endif

21
source/qFix/qfixI2CDefs.h Normal file
View file

@ -0,0 +1,21 @@
//------------------------------------------------------------------
// qfixI2CDefs.h
//
// This file contains definitions for the qfix I2C protocol.
//
// Copyright 2005 by KTB mechatronics GmbH
// Author: Stefan Enderle
//------------------------------------------------------------------
#ifndef qfixI2CDefs_h
#define qfixI2CDefs_h
const uint8_t I2C_ADDR_BOBBYBOARD = 1;
const uint8_t I2C_ADDR_LCD = 2;
#endif

119
source/qFix/qfixI2CMaster.h Normal file
View file

@ -0,0 +1,119 @@
//------------------------------------------------------------------
// qfixI2CMaster.h
//
// This is a convenience class for writing I2C master classes
// (like SlaveBoard).
// This class supports the qfix I2C protocol.
//
// Copyright 2005 by KTB mechatronics GmbH
// Author: Stefan Enderle
//------------------------------------------------------------------
#ifndef qfixI2CMaster_h
#define qfixI2CMaster_h
#include "qfixI2C.h"
#include "qfixI2CDefs.h"
const uint8_t CMD_PARAMS = 255; // the same for all boards
//---------------------------------------------------------------
class I2CMaster
{
I2C i2c;
uint8_t brdID; // board ID (what board)
uint8_t logID; // logical ID (which one of these boards)
public:
/** boardID denotes the board (BobbyBoard, LCD, ...) that
* shall be used. The logical ID is set to 0.
*/
I2CMaster(uint8_t boardID);
/** boardID denotes the board (BobbyBoard, LCD, ...) that
* shall be used. logicalID is the sub-ID (e.g. which LCD).
*/
I2CMaster(uint8_t boardID, uint8_t logicalID);
void command(uint8_t cmd);
void command(uint8_t cmd, uint8_t param);
void command(uint8_t cmd, uint8_t param, uint8_t param2);
void command(uint8_t cmd, uint8_t param, uint8_t param2, uint8_t param3);
uint8_t request(uint8_t cmd, uint8_t param);
};
I2CMaster::I2CMaster(uint8_t boardID)
: i2c(), brdID(boardID), logID(0)
{
}
I2CMaster::I2CMaster(uint8_t boardID, uint8_t logicalID)
: i2c(), brdID(boardID), logID(logicalID)
{
}
void I2CMaster::command(uint8_t cmd)
{
uint8_t buf[2];
buf[0] = logID;
buf[1] = cmd;
i2c.send(brdID, buf, 2);
}
void I2CMaster::command(uint8_t cmd, uint8_t param)
{
uint8_t buf[3];
buf[0] = logID;
buf[1] = cmd;
buf[2] = param;
i2c.send(brdID, buf, 3);
}
void I2CMaster::command(uint8_t cmd, uint8_t param, uint8_t param2)
{
uint8_t buf[4];
buf[0] = logID;
buf[1] = cmd;
buf[2] = param;
buf[3] = param2;
i2c.send(brdID, buf, 4);
}
void I2CMaster::command(uint8_t cmd, uint8_t param, uint8_t param2, uint8_t param3)
{
uint8_t buf[5];
buf[0] = logID;
buf[1] = cmd;
buf[2] = param;
buf[3] = param2;
buf[4] = param3;
i2c.send(brdID, buf, 5);
}
uint8_t I2CMaster::request(uint8_t cmd, uint8_t param)
{
// send command and params first //
command(CMD_PARAMS, cmd, param);
// get return value //
uint8_t buf[10];
i2c.get(brdID, buf, 1);
return(buf[0]);
}
#endif

113
source/qFix/qfixI2CSlave.h Normal file
View file

@ -0,0 +1,113 @@
//------------------------------------------------------------------
// qfixI2CSlave.h
//
// This is a convenience class for writing I2C slave programs
// (like slaveBoard.cc).
// This class supports the qfix I2C protocol.
//
// Copyright 2005 by KTB mechatronics GmbH
// Author: Stefan Enderle
//------------------------------------------------------------------
#ifndef qfixI2CSlave_h
#define qfixI2CSlave_h
#include "qfixI2C.h"
#include "qfixI2CDefs.h"
#include "qfixI2CMaster.h" // Only for constants
//---------------------------------------------------------------
class I2CSlave
{
public:
I2C i2c;
uint8_t brdID; // board ID (what board)
uint8_t logID; // logical ID (which one of these boards)
void (*commandPtr)(uint8_t cmd, uint8_t* params, uint8_t len);
void (*requestPtr)(uint8_t cmd, uint8_t* params, uint8_t len);
uint8_t params[100];
uint8_t data[100];
public:
/** boardID denotes the board (BobbyBoard, LCD, ...) that
* shall be used. The logical ID is set to 0.
*/
I2CSlave(uint8_t boardID);
/** boardID denotes the board (BobbyBoard, LCD, ...) that
* shall be used. logicalID is the sub-ID (e.g. which LCD).
*/
I2CSlave(uint8_t boardID, uint8_t logicalID);
void setLogicalID(uint8_t logicalID);
void run();
};
I2CSlave::I2CSlave(uint8_t boardID)
: i2c(), brdID(boardID), logID(0)
{
i2c.setSlaveAdress(boardID);
}
I2CSlave::I2CSlave(uint8_t boardID, uint8_t logicalID)
: i2c(), brdID(boardID), logID(logicalID)
{
i2c.setSlaveAdress(boardID);
}
void I2CSlave::setLogicalID(uint8_t logicalID)
{
logID = logicalID;
}
void I2CSlave::run()
{
while (1) {
// wait for action on the I2C bus //
while (!i2c.isAction()) { }
// was it a command (send) //
if (i2c.isActionSend()) {
// receive bytes: 0=logical ID, 1=command, 2..n=params
int len = i2c.receive(data);
// if logical ID matches (or 255) -> execute //
if ( (data[0] == logID) || (data[0]==255) ) {
// is it the first part of a request ? -> save params //
if (data[1] == CMD_PARAMS) {
for (int i=0; i<len; i++) {
params[i] = data[i];
}
}
// it is a command -> call the given command function //
else commandPtr(data[1], data+2, len-2);
}
}
// or was it a request (get) //
else if (i2c.isActionGet()) {
requestPtr(params[2], params+3, 0); // call given rqs function
}
// else ERROR !
}
}
#endif

216
source/qFix/qfixLCD.h Normal file
View file

@ -0,0 +1,216 @@
//------------------------------------------------------------------
// qfixLCD.h
//
// This file contains the class LCD which represents the
// LCD display.
//
// Copyright 2004 by KTB mechatronics GmbH
// Author: Stefan Enderle
//------------------------------------------------------------------
#ifndef qfixLCD_h
#define qfixLCD_h
#include "qfixI2C.h"
#include "qfixI2CDefs.h"
#include "string.h"
static const uint8_t LCD_CMD_CHANGEID = 0;
static const uint8_t LCD_CMD_CLEAR = 1;
static const uint8_t LCD_CMD_PRINTSTR = 2;
static const uint8_t LCD_CMD_PRINTSTRXY = 3;
static const uint8_t LCD_CMD_LIGHTON = 4;
static const uint8_t LCD_CMD_LIGHTOFF = 5;
static const uint8_t LCD_CMD_PRINTINT = 6;
static const uint8_t LCD_CMD_PRINTINTXY = 7;
/**
* \class LCD
* \brief Represents the qfix LC-display.
* \author Stefan Enderle
*
* The class LCD represents the
* physical LC-display and can be used to output text.
* The I2C bus is completely abstracted.
*/
class LCD
{
private:
I2C i2c;
uint8_t id;
public:
/** Default constructor. If only one LC-display is used,
* this constructor can be used. It sets the internal ID to 0.
*/
LCD();
/** Constructor with given ID.
* If two or more LC-displays are used,
* this constructor must be used. It sets the internal ID to the given value.
*/
LCD(uint8_t ID);
/** Change logical device ID
*/
void changeID(uint8_t newID);
/** Clears the display
*/
void clear();
/** prints the string data at the current cursor position.
* The string must end with 0.
*/
void print(char* data);
/** Prints the string data at position line/col. The string
* must end with 0.
*/
void print(uint8_t line, uint8_t col, char* data);
/** prints the integer data at the current cursor position.
*/
void print(int data);
/** Prints the integer data at position line/col.
*/
void print(uint8_t line, uint8_t col, int data);
/** Puts on the backlight.
* \see lightOff
*/
void lightOn();
/** Puts off the backlight.
* \see lightOff
*/
void lightOff();
};
LCD::LCD() : i2c(), id(0)
{
}
LCD::LCD(uint8_t ID) : i2c(), id(ID)
{
}
void LCD::clear()
{
uint8_t buf[2];
buf[0] = id;
buf[1] = LCD_CMD_CLEAR;
i2c.send(I2C_ADDR_LCD, buf, 2);
}
void LCD::print(char* data)
{
int len=strlen(data);
uint8_t buf[len+2];
buf[0] = id;
buf[1] = LCD_CMD_PRINTSTR;
for (int i=0; i<len; i++) buf[i+2] = uint8_t(data[i]);
i2c.send(I2C_ADDR_LCD, buf, len+2);
}
void LCD::print(uint8_t line, uint8_t col, char* data)
{
int len=strlen(data);
uint8_t buf[len+4];
buf[0] = id;
buf[1] = LCD_CMD_PRINTSTRXY;
buf[2] = line;
buf[3] = col;
for (int i=0; i<len; i++) buf[i+4] = uint8_t(data[i]);
i2c.send(I2C_ADDR_LCD, buf, len+4);
}
void LCD::print(int data)
{
int len=sizeof(int);
uint8_t buf[len+2];
uint8_t* ptr = (uint8_t*) &data; // pointer to data
buf[0] = id;
buf[1] = LCD_CMD_PRINTINT;
for (int i=0; i<len; i++) {
buf[i+2] = *ptr;
ptr++;
}
i2c.send(I2C_ADDR_LCD, buf, len+2);
}
void LCD::print(uint8_t line, uint8_t col, int data)
{
int len=sizeof(int);
uint8_t buf[len+4];
uint8_t* ptr = (uint8_t*) &data; // pointer to data
buf[0] = id;
buf[1] = LCD_CMD_PRINTINTXY;
buf[2] = line;
buf[3] = col;
for (int i=0; i<len; i++) {
buf[i+4] = *ptr;
ptr++;
}
i2c.send(I2C_ADDR_LCD, buf, len+4);
}
void LCD::lightOn()
{
uint8_t buf[2];
buf[0] = id;
buf[1] = LCD_CMD_LIGHTON;
i2c.send(I2C_ADDR_LCD, buf, 2);
}
void LCD::lightOff()
{
uint8_t buf[2];
buf[0] = id;
buf[1] = LCD_CMD_LIGHTOFF;
i2c.send(I2C_ADDR_LCD, buf, 2);
}
void LCD::changeID(uint8_t newID)
{
uint8_t buf[3];
buf[0] = id;
buf[1] = LCD_CMD_CHANGEID;
buf[2] = newID;
i2c.send(I2C_ADDR_LCD, buf, 3);
id = newID;
}
#endif

323
source/qFix/qfixMega128Board.h Normal file
View file

@ -0,0 +1,323 @@
//------------------------------------------------------------------
// qfixMega128Board.h
//
// This file contains the class Mega128Board which represents the
// physical Mega128Board with all its inputs and outputs.
//
// Copyright 2005 by KTB mechatronics GmbH
// Author: Stefan Enderle, Florian Schrapp
//------------------------------------------------------------------
#include "qfix.h"
#ifndef qfixMega128Board_h
#define qfixMega128Board_h
static int speedMotor0 = 50;
static int speedMotor1 = 50;
class Mega128Board
{
public:
/** Constructor for the robot board class.
*/
Mega128Board();
/** Puts on LED i
*/
void ledOn(int i);
/** Puts off LED i
*/
void ledOff(int i);
/** Puts off all LEDs
*/
void ledsOff();
/** Puts LED i on if state is true, else off
*/
void led(int i, bool state);
/** Uses the four LEDs on the board to display the value i
* with 0 <= i <= 255
*/
void ledMeter(int i);
/** Puts the power output i on
*/
void powerOn(int i);
/** Puts the power output i off
*/
void powerOff(int i);
/** Puts the power output i on if state is true, else off
*/
void power(int i, bool state);
/** Checks the state of button i. If it is pressed, true is returned,
* else false.
*/
bool button(int i);
/** Sets motor i to the given speed. -255 <= speed <= 255
*/
void motor(int i, int speed);
/** Sets both motors to the given speed. -255 <= speed <= 255
*/
void motors(int speed0, int speed1);
/** Puts off both motors.
*/
void motorsOff();
/** returns the value of the analog port i. 0 <= value <= 255
*/
int analog(int i);
/** returns true if the digital port is logical high, else false.
*/
bool digital(int i);
};
// PWM routine //
SIGNAL (SIG_OVERFLOW0)
{
const int OFFSET=50; // motor does not work with very low ratio
static int counter=255+OFFSET;
if (speedMotor1==0) cbi(PORTB, 6); // enable1 = 0
else if (abs(speedMotor1)+OFFSET >= counter) sbi(PORTB, 6); // enable1 = 1
else cbi(PORTB, 6); // enable1 = 0
if (speedMotor0==0) cbi(PORTB, 7); // enable2 = 0
else if (abs(speedMotor0)+OFFSET >= counter) sbi(PORTB, 7); // enable2 = 1
else cbi(PORTB, 7); // enable2 = 0
if (counter==0) counter=255+OFFSET;
else counter--;
}
void initTimer()
{
TCCR0=1; // timer 0 for interrupt
TIMSK=1;
sei(); // enable interrupts
}
Mega128Board::Mega128Board()
{
// Port A: motors input
DDRA = 4+8+16+64; // all motors output
PORTA = 0; // clear all bits = motors off
// Port B: motors enable
DDRB = 64+128; // upper bits output
PORTB = 0; // clear bit 6 and 7 = motors off
// Port C: power output //
DDRC = 255; // default direction port C: all bits output
PORTC = 0; // clear all bits: power on
// Port D: I2C //
DDRD = 0; // all bits input
PORTD = 1+2; // set bits 0,1 -> I2C pullUps
// Port E: leds + buttons //
DDRE = 4+8+16+32; // pin 2-5 leds output; pin 6+7 input for buttons 0+1
PORTE |= 4+8+16+32; // set leds off
PORTE |= 64+128; // set pullups for buttons 0+1
// Port F: analog in + digital in //
DDRF = 0; // all bits input
// Port G: buttons //
DDRG = 0; // pin 3+4 input for buttons 2+3
PORTG |= 8+16; // set pullups for buttons 3+4
initTimer();
}
void Mega128Board::ledOn(int i)
{
if ((i<0) || (i>3)) return;
cbi(PORTE, i+2); // clear bit -> LED on
}
void Mega128Board::ledOff(int i)
{
if ((i<0) || (i>3)) return;
sbi(PORTE, i+2); // set bit -> LED off
}
void Mega128Board::ledsOff()
{
ledOff(0);
ledOff(1);
ledOff(2);
ledOff(3);
}
void Mega128Board::led(int i, bool state)
{
if (state) ledOn(i); else ledOff(i);
}
void Mega128Board::ledMeter(int i)
{
led(0, (i>50));
led(1, (i>100));
led(2, (i>150));
led(3, (i>200));
}
void Mega128Board::powerOn(int i)
{
if ((i<0) || (i>7)) return;
cbi(PORTC, i);
}
void Mega128Board::powerOff(int i)
{
if ((i<0) || (i>7)) return;
sbi(PORTC, i);
}
void Mega128Board::power(int i, bool state)
{
if (state) powerOn(i); else powerOff(i);
}
bool Mega128Board::button(int i)
{
if ((i<0) || (i>3)) return false;
else if (i==0 || i ==1){
return ((PINE & (64<<i)) == 0); //wenn pina=0 button gedrückt => 0&1=false, also 0==0 => true
}
else if (i==2 || i==3){
return ((PING & (2<<i)) == 0);
}
else return false;
}
void Mega128Board::motor(int i, int speed)
{
if ((i<0) || (i>1)) return;
if (i==1) {
speedMotor1 = speed;
if (speed==0) {
cbi(PORTA, 2); // input1 = 0
cbi(PORTA, 3); // input2 = 0
//cbi(PORTB, 6); // enable1 = 0
}
else if (speed>0) {
sbi(PORTA, 2); // input1 = 1
cbi(PORTA, 3); // input2 = 0
//sbi(PORTB, 6); // enable1 = 1
}
else {
cbi(PORTA, 2); // input1 = 0
sbi(PORTA, 3); // input2 = 1
}
}
if (i==0) {
speedMotor0 = speed;
if (speed==0) {
cbi(PORTA, 4); // input3 = 0
cbi(PORTA, 5); // input4 = 0
//cbi(PORTB, 7); // enable2 = 0
}
else if (speed>0){
sbi(PORTA, 4); // input3 = 1
cbi(PORTA, 5); // input4 = 0
//sbi(PORTB, 7); // enable2 = 1
}
else {
cbi(PORTA, 4); // input3 = 0
sbi(PORTA, 5); // input4 = 1
}
}
}
void Mega128Board::motors(int motor0, int motor1)
{
motor(0,motor0);
motor(1,motor1);
}
void Mega128Board::motorsOff()
{
motor(0,0);
motor(1,0);
}
// return 0-255 //
int Mega128Board::analog(int i)
{
if ((i<0) || (i>7)) return -1;
ADCSRA=128; // set A/D enable bit (ADEN)
ADMUX=i; // select analog input and start A/D
sbi(ADMUX, ADLAR); // left adjust -> we use only ADCH
sbi(ADCSRA, ADSC); // start conversion
while (ADCSRA & 64); // wait until ADSC is low again
int value = ADCH; // read 8 bit value fom ADCH
return value;
}
bool Mega128Board::digital(int i)
{
if ((i<0) || (i>7)) return false; // bad solution...
ADCSRA=0; // clear A/D enable bit (ADEN)
PORTF |= (1<<i); // set pullups for digital inputs
return (PINF & (1<<i)) ;
}
#endif

89
source/qFix/qfixServoBoard4.h Normal file
View file

@ -0,0 +1,89 @@
//------------------------------------------------------------------
// qfixServoBoard4.h
//
// This file contains the class ServoBoard4 which represents
// four servos attached to the ServoBoard4.
//
// Copyright 2006 by KTB mechatronics GmbH
// Author: Stefan Enderle
//------------------------------------------------------------------
#ifndef qfixServoBoard4_h
#define qfixServoBoard4_h
#include "qfixI2C.h"
#include "qfixI2CDefs.h"
#include "string.h"
static const uint8_t I2C_ADDR_SERVO = 3;
static const uint8_t SERVO_CMD_CHANGEID = 0;
static const uint8_t SERVO_CMD_SET = 1;
class ServoBoard4
{
private:
I2C i2c;
uint8_t id;
public:
/** Constructor with default ID 0
*/
ServoBoard4();
/** Constructor with given ID
*/
ServoBoard4(uint8_t ID);
/** Change logical device ID
*/
void changeID(uint8_t newID);
/**
*/
void set(uint8_t index, uint8_t pwm);
};
ServoBoard4::ServoBoard4() : i2c(), id(0)
{
}
ServoBoard4::ServoBoard4(uint8_t ID) : i2c(), id(ID)
{
}
void ServoBoard4::set(uint8_t index, uint8_t pwm)
{
uint8_t buf[5];
buf[0] = id;
buf[1] = SERVO_CMD_SET;
buf[2] = index;
buf[3] = pwm;
i2c.send(I2C_ADDR_SERVO, buf, 4);
}
void ServoBoard4::changeID(uint8_t newID)
{
uint8_t buf[3];
buf[0] = id;
buf[1] = SERVO_CMD_CHANGEID;
buf[2] = newID;
i2c.send(I2C_ADDR_SERVO, buf, 3);
id = newID;
}
#endif

157
source/qFix/qfixSlaveBoard.h Normal file
View file

@ -0,0 +1,157 @@
//------------------------------------------------------------------
// qfixSlaveBoard.h
//
// This file contains the class SlaveBoard which represents a kind
// of remote control of a BobbyBoard via the I2C bus. The SlaveBoard
// can be used like a local BobbyBoard with all its inputs and outputs.
//
// Copyright 2005 by KTB mechatronics GmbH
// Author: Stefan Enderle
//------------------------------------------------------------------
#ifndef qfixSlaveBoard_h
#define qfixSlaveBoard_h
#include "qfixI2CMaster.h"
#include "qfixI2CDefs.h"
const uint8_t CMD_LEDON = 0;
const uint8_t CMD_LEDOFF = 1;
const uint8_t CMD_LEDSOFF = 2;
const uint8_t CMD_POWERON = 3;
const uint8_t CMD_POWEROFF = 4;
const uint8_t CMD_MOTOR = 5;
const uint8_t CMD_BUTTON = 6;
const uint8_t CMD_ANALOG = 7;
const uint8_t CMD_DIGITAL = 8;
//----------------------------------------
class SlaveBoard : I2CMaster
{
public:
SlaveBoard();
SlaveBoard(uint8_t logicalID);
/** Puts on LED i
*/
void ledOn(uint8_t i);
/** Puts off LED i
*/
void ledOff(uint8_t i);
/** Puts off all LEDs
*/
void ledsOff();
/** Puts the power output i on
*/
void powerOn(uint8_t i);
/** Puts the power output i off
*/
void powerOff(uint8_t i);
/** Sets motor i to the given speed. -255 <= speed <= 255
*/
void motor(uint8_t i, int speed);
/** Checks the state of button i. If it is pressed, true is returned,
* else false.
*/
bool button(uint8_t i);
/** returns the value of the analog port i. 0 <= value <= 255
*/
int analog(uint8_t i);
/** returns true if the digital port is logical high, else false.
*/
bool digital(uint8_t i);
};
SlaveBoard::SlaveBoard() : I2CMaster(I2C_ADDR_BOBBYBOARD)
{
}
SlaveBoard::SlaveBoard(uint8_t logicalID)
: I2CMaster(I2C_ADDR_BOBBYBOARD, logicalID)
{
}
void SlaveBoard::ledOn(uint8_t i)
{
if (i>3) return;
command(CMD_LEDON, i);
}
void SlaveBoard::ledOff(uint8_t i)
{
if (i>3) return;
command(CMD_LEDOFF, i);
}
void SlaveBoard::ledsOff()
{
command(CMD_LEDSOFF);
}
void SlaveBoard::powerOn(uint8_t i)
{
if (i>7) return;
command(CMD_POWERON, i);
}
void SlaveBoard::powerOff(uint8_t i)
{
if (i>7) return;
command(CMD_POWEROFF, i);
}
void SlaveBoard::motor(uint8_t i, int speed)
{
if (i>1) return;
uint8_t neg=(speed<0)?1:0;
uint8_t s;
if (speed>=0) s=uint8_t(speed); else s=-uint8_t(speed);
command(CMD_MOTOR, i, neg, s);
}
bool SlaveBoard::button(uint8_t i)
{
if (i>3) return false; // better error handling!
return (request(CMD_BUTTON, i) == 1);
}
int SlaveBoard::analog(uint8_t i)
{
if (i>3) return -1; // better error handling!
return (request(CMD_ANALOG, i));
}
bool SlaveBoard::digital(uint8_t i)
{
if (i>3) return false; // better error handling!
return (request(CMD_DIGITAL, i) == 1);
}
#endif

339
source/qFix/qfixSoccerBoard.h Normal file
View file

@ -0,0 +1,339 @@
//------------------------------------------------------------------
// qfixSoccerBoard.h
//
// This file contains the class SoccerBoard which represents the
// physical SoccerBoard with all its inputs and outputs.
//
// Copyright 2004-2006 by KTB mechatronics GmbH
// Author: Stefan Enderle
//------------------------------------------------------------------
#include "qfix.h"
#ifndef qfixSoccerBoard_h
#define qfixSoccerBoard_h
static int speedMotor0 = 0;
static int speedMotor1 = 0;
static int speedMotor2 = 0;
static int speedMotor3 = 0;
static int speedMotor4 = 0;
static int speedMotor5 = 0;
/**
* \class SoccerBoard
* \brief Represents the controller board "SoccerBoard".
* \author Stefan Enderle
*
* The class SoccerBoard represents the
* physical SoccerBoard with all its inputs and outputs.
* With this class it is possible to drive the motors,
* put on LEDs, check the buttons and get data from the
* analog and digital inputs.
*/
class SoccerBoard
{
public:
/** Constructor for the SoccerBoard class.
*/
SoccerBoard();
/** Puts on LED i. i must be 0 or 1.
*/
void ledOn(int i);
/** Puts off LED i. i must be 0 or 1.
*/
void ledOff(int i);
/** Puts off all LEDs.
*/
void ledsOff();
/** Puts LED i on if state is true, else off. i must be 0 or 1.
*/
void led(int i, bool state);
/** Puts the power output i on
*/
void powerOn(int i);
/** Puts the power output i off
*/
void powerOff(int i);
/** Puts the power output i on if state is true, else off.
*/
void power(int i, bool state);
/** Checks the state of button i. If it is pressed, true is returned,
* else false.
*/
bool button(int i);
/** Uses the four LEDs on the board to display the value i
* with 0 <= i <= 255
*/
void ledMeter(int i);
/** Sets motor i to the given speed. -255 <= speed <= 255.
*/
void motor(int i, int speed);
/** Puts off all motors.
*/
void motorsOff();
/** returns the value of the analog port i. 0 <= value <= 255.
*/
int analog(int i);
/** returns true if the digital port is logical high, else false.
*/
bool digital(int i);
/** Waits until button i is pressed and released again.
*/
void waitForButton(int i);
};
// PWM routine //
SIGNAL (SIG_OVERFLOW0)
{
const int OFFSET=50; // motor does not work with very low ratio
static int counter=255+OFFSET;
if (speedMotor0==0) cbi(PORTB, 3); // enable1 = 0
else if (abs(speedMotor0)+OFFSET >= counter) sbi(PORTB, 3); // enable1 = 1
else cbi(PORTB, 3); // enable1 = 0
if (speedMotor1==0) cbi(PORTG, 2); // enable2 = 0
else if (abs(speedMotor1)+OFFSET >= counter) sbi(PORTG, 2); // enable2 = 1
else cbi(PORTG, 2); // enable2 = 0
if (speedMotor2==0) cbi(PORTB, 4); // enable3 = 0
else if (abs(speedMotor2)+OFFSET >= counter) sbi(PORTB, 4); // enable3 = 1
else cbi(PORTB, 4); // enable3 = 0
if (speedMotor3==0) cbi(PORTB, 6); // enable4 = 0
else if (abs(speedMotor3)+OFFSET >= counter) sbi(PORTB, 6); // enable4 = 1
else cbi(PORTB, 6); // enable4 = 0
if (speedMotor4==0) cbi(PORTB, 5); // enable5 = 0
else if (abs(speedMotor4)+OFFSET >= counter) sbi(PORTB, 5); // enable5 = 1
else cbi(PORTB, 5); // enable5 = 0
if (speedMotor5==0) cbi(PORTB, 7); // enable6 = 0
else if (abs(speedMotor5)+OFFSET >= counter) sbi(PORTB, 7); // enable6 = 1
else cbi(PORTB, 7); // enable6 = 0
if (counter==0) counter=255+OFFSET;
else counter--;
}
void initTimer()
{
TCCR0A=1; // timer 0 for interrupt
TIMSK0=1;
sei(); // enable interrupts
}
SoccerBoard::SoccerBoard()
{
// PORT A: Digital In (PA0, PA1) //
DDRA= 255 - (1+2); // all bits output (motor direction) except PA0,PA1 (digital in)
PORTA=1+2; // set pullups for digital in
// PORT B: LEDs (PB0, PB2), Motor enable (PB3-PB7) //
DDRB = 1+4; // PB0 + PB4 = LEDs -> output
DDRB |= 8+16+32+64+128; // PB3 - PB7 = Motor enable -> output
PORTB |= 1+4; // set bits 0 and 2 -> LEDs off
PORTB &= 255-(8+16+32+64+128); // clear bits 3-7 -> motor disable
// PORT C: Power Output //
DDRC = 255; // direction port D, all bits output
PORTC = 0; // clear all bits -> power on
// PORT D: I2C, USB, CAN //
DDRD = 0; // all bits input
PORTD = 1+2; // set bits 0,1 -> I2C pullUps
// PORT E: Digital In (PE2 - PE7) //
DDRE=0; // all bits input
PORTE=4+8+16+32+64+128; // set pullups for digital in
// PORT F: Analog In //
DDRF=0; // all bits input
ADCSRA=128; // set A/D enable bit (ADEN)
// PORT G: Buttons (PG3, PG4), motor enable (PG2)
DDRG = BV(PG2); // PG2 output
PORTG= BV(PG3)+BV(PG4); // set pullups for buttons
cbi(PORTG,PG2); // clear PG2 -> motor disable
initTimer();
}
void SoccerBoard::ledOn(int i)
{
if (i==0) cbi(PORTB, PB0); // clear bit -> LED on
else if (i==1) cbi(PORTB, PB2); // clear bit -> LED on
}
void SoccerBoard::ledOff(int i)
{
if (i==0) sbi(PORTB, PB0); // set bit -> LED off
else if (i==1) sbi(PORTB, PB2); // set bit -> LED off
}
void SoccerBoard::ledsOff()
{
PORTB|=BV(PB0)+BV(PB2); // set bits -> LEDs off
}
void SoccerBoard::led(int i, bool state)
{
if (state) ledOn(i); else ledOff(i);
}
void SoccerBoard::powerOn(int i)
{
if ((i<0) || (i>7)) return;
cbi(PORTC, i);
}
void SoccerBoard::powerOff(int i)
{
if ((i<0) || (i>7)) return;
sbi(PORTC, i);
}
void SoccerBoard::power(int i, bool state)
{
if (state) powerOn(i); else powerOff(i);
}
bool SoccerBoard::button(int i)
{
if (i==0) return ( (PING & BV(PG4)) == 0);
else if (i==1) return ( (PING & BV(PG3)) == 0);
else return false; // bad approach...
}
void SoccerBoard::ledMeter(int i)
{
led(0, (i>100));
led(1, (i>200));
}
void SoccerBoard::motor(int i, int speed)
{
if ((i<0) || (i>5)) return;
if (i==0) {
speedMotor0 = speed;
if (speed>0) sbi(PORTA, 3); // input1 = 1
else cbi(PORTA, 3); // input1 = 0
}
else if (i==1) {
speedMotor1 = speed;
if (speed>0) sbi(PORTA, 2); // input2 = 1
else cbi(PORTA, 2); // input2 = 0
}
else if (i==2) {
speedMotor2 = speed;
if (speed>0) sbi(PORTA, 5); // input3 = 1
else cbi(PORTA, 5); // input3 = 0
}
else if (i==3) {
speedMotor3 = speed;
if (speed>0) sbi(PORTA, 4); // input4 = 1
else cbi(PORTA, 4); // input4 = 0
}
else if (i==4) {
speedMotor4 = speed;
if (speed>0) sbi(PORTA, 7); // input5 = 1
else cbi(PORTA, 7); // input5 = 0
}
else if (i==5) {
speedMotor5 = speed;
if (speed>0) sbi(PORTA, 6); // input6 = 1
else cbi(PORTA, 6); // input6 = 0
}
}
void SoccerBoard::motorsOff()
{
motor(0,0);
motor(1,0);
motor(2,0);
motor(3,0);
motor(4,0);
motor(5,0);
}
// return 0-255 //
int SoccerBoard::analog(int i)
{
if ((i<0) || (i>7)) return -1;
ADMUX=i; // select analog input and start A/D
sbi(ADMUX, ADLAR); // left adjust -> we use only ADCH
sbi(ADCSRA, ADSC); // start conversion
while (ADCSRA & 64); // wait until ADSC is low again
int value = ADCH; // read 8 bit value fom ADCH
return value;
}
bool SoccerBoard::digital(int i)
{
if ((i<0) || (i>7)) return false; // bad solution...
if (i==0) return (PINA & 1);
else if (i==1) return (PINA & 2);
else return (PINE & (1<<i)) ;
}
void SoccerBoard::waitForButton(int i)
{
if ((i<0) || (i>3)) return; // bad solution...
while (!button(i)) { /* do nothing */ }
while (button(i)) { /* do nothing */ }
}
#endif

107
source/qFix/qfixSonar.h Normal file
View file

@ -0,0 +1,107 @@
//------------------------------------------------------------------
// qfixSonar.h
//
// This file contains the class Sonar which represents a
// sonar modul SFH04/08.
//
// Copyright 2005 by KTB mechatronics GmbH
// Author: Florian Schrapp, Stefan Enderle
// Version 1.0
//------------------------------------------------------------------
#ifndef qfixSonar_h
#define qfixSonar_h
#include "qfix.h"
#include "qfixI2C.h"
class Sonar
{
private:
uint8_t id;
I2C i2c;
public:
/** Constructor with given ID
*/
Sonar(uint8_t ID);
/** Change logical device ID
*/
void changeID(uint8_t newID);
/** Returns the distance to the next object in millimeter
*/
int distance();
};
Sonar::Sonar(uint8_t ID) : i2c()
{
id = 112+ID;
uint8_t buf[10];
buf[0]=2; // register 2 = select range register
buf[1]=48; // set maximum range to 2107 mm (formula: range=(register*43)+43 mm)
i2c.send(id, buf, 2);
buf[0]=1; // register 1 = amplification
buf[1]=4; // improve detection along center axis
i2c.send(id, buf, 2);
}
void Sonar::changeID(uint8_t newID)
{
if(newID>=0 && newID<=15){ // only 16 IDs allowed
uint8_t buf[10];
buf[0]=0;
buf[1]=160;
i2c.send(id, buf, 2);
buf[0]=0;
buf[1]=170;
i2c.send(id, buf, 2);
buf[0]=0;
buf[1]=165;
i2c.send(id, buf, 2);
buf[0]=0;
buf[1]=(newID*2)+224;
i2c.send(id, buf, 2);
id=112+newID;
} //else error!
}
int Sonar::distance()
{
uint8_t buf[10];
buf[0]=0; // start measurement
buf[1]=81;
i2c.send(id, buf, 2);
msleep(70); // wait for echo!!! (about 65ms)
i2c.send(id, 3); // select byte to be read
i2c.get(id, buf, 1); // read first byte
uint8_t r1=buf[0];
i2c.send(id, 2); // select byte to be read
i2c.get(id, buf, 1); // read second byte
uint8_t r2=buf[0];
return ((r1+(256*r2))*10); // compute mm
}
#endif