From 56d9bdd39ed36c36e9a61411b86c76d5228b2133 Mon Sep 17 00:00:00 2001 From: sicarius Date: Sun, 11 Feb 2007 18:32:03 +0000 Subject: Added lot's of code-files used during work --- source/qFix/qfix.h | 96 ++++++++++++ source/qFix/qfixBobbyBoard.h | 344 +++++++++++++++++++++++++++++++++++++++++ source/qFix/qfixCAN128Board.h | 322 ++++++++++++++++++++++++++++++++++++++ source/qFix/qfixI2C.h | 278 +++++++++++++++++++++++++++++++++ source/qFix/qfixI2CDefs.h | 21 +++ source/qFix/qfixI2CMaster.h | 119 ++++++++++++++ source/qFix/qfixI2CSlave.h | 113 ++++++++++++++ source/qFix/qfixLCD.h | 216 ++++++++++++++++++++++++++ source/qFix/qfixMega128Board.h | 323 ++++++++++++++++++++++++++++++++++++++ source/qFix/qfixServoBoard4.h | 89 +++++++++++ source/qFix/qfixSlaveBoard.h | 157 +++++++++++++++++++ source/qFix/qfixSoccerBoard.h | 339 ++++++++++++++++++++++++++++++++++++++++ source/qFix/qfixSonar.h | 107 +++++++++++++ 13 files changed, 2524 insertions(+) create mode 100644 source/qFix/qfix.h create mode 100644 source/qFix/qfixBobbyBoard.h create mode 100644 source/qFix/qfixCAN128Board.h create mode 100644 source/qFix/qfixI2C.h create mode 100644 source/qFix/qfixI2CDefs.h create mode 100644 source/qFix/qfixI2CMaster.h create mode 100644 source/qFix/qfixI2CSlave.h create mode 100644 source/qFix/qfixLCD.h create mode 100644 source/qFix/qfixMega128Board.h create mode 100644 source/qFix/qfixServoBoard4.h create mode 100644 source/qFix/qfixSlaveBoard.h create mode 100644 source/qFix/qfixSoccerBoard.h create mode 100644 source/qFix/qfixSonar.h (limited to 'source/qFix') diff --git a/source/qFix/qfix.h b/source/qFix/qfix.h new file mode 100644 index 0000000..0a5d039 --- /dev/null +++ b/source/qFix/qfix.h @@ -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 +#include +#include + + +/* 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=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 + diff --git a/source/qFix/qfixBobbyBoard.h b/source/qFix/qfixBobbyBoard.h new file mode 100644 index 0000000..c2f04a4 --- /dev/null +++ b/source/qFix/qfixBobbyBoard.h @@ -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<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<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 + diff --git a/source/qFix/qfixCAN128Board.h b/source/qFix/qfixCAN128Board.h new file mode 100644 index 0000000..a10b288 --- /dev/null +++ b/source/qFix/qfixCAN128Board.h @@ -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< 0&1=false, also 0==0 => true + } + else if (i==2 || i==3){ + return ((PING & (2<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< 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< 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 OK + TWCR = (1< OK + if ((TWSR&248)==0xC0); // data sent, ACK not received -> ERROR + } + TWCR = (1< 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 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 + diff --git a/source/qFix/qfixLCD.h b/source/qFix/qfixLCD.h new file mode 100644 index 0000000..9ecd93f --- /dev/null +++ b/source/qFix/qfixLCD.h @@ -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= 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< 0&1=false, also 0==0 => true + } + else if (i==2 || i==3){ + return ((PING & (2<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<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 + diff --git a/source/qFix/qfixSoccerBoard.h b/source/qFix/qfixSoccerBoard.h new file mode 100644 index 0000000..96570bd --- /dev/null +++ b/source/qFix/qfixSoccerBoard.h @@ -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<3)) return; // bad solution... + while (!button(i)) { /* do nothing */ } + while (button(i)) { /* do nothing */ } +} + + +#endif + diff --git a/source/qFix/qfixSonar.h b/source/qFix/qfixSonar.h new file mode 100644 index 0000000..96d26ce --- /dev/null +++ b/source/qFix/qfixSonar.h @@ -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 \ No newline at end of file -- cgit v1.2.3