diff options
Diffstat (limited to 'source/qFix')
| -rw-r--r-- | source/qFix/qfix.h | 96 | ||||
| -rw-r--r-- | source/qFix/qfixBobbyBoard.h | 344 | ||||
| -rw-r--r-- | source/qFix/qfixCAN128Board.h | 322 | ||||
| -rw-r--r-- | source/qFix/qfixI2C.h | 278 | ||||
| -rw-r--r-- | source/qFix/qfixI2CDefs.h | 21 | ||||
| -rw-r--r-- | source/qFix/qfixI2CMaster.h | 119 | ||||
| -rw-r--r-- | source/qFix/qfixI2CSlave.h | 113 | ||||
| -rw-r--r-- | source/qFix/qfixLCD.h | 216 | ||||
| -rw-r--r-- | source/qFix/qfixMega128Board.h | 323 | ||||
| -rw-r--r-- | source/qFix/qfixServoBoard4.h | 89 | ||||
| -rw-r--r-- | source/qFix/qfixSlaveBoard.h | 157 | ||||
| -rw-r--r-- | source/qFix/qfixSoccerBoard.h | 339 | ||||
| -rw-r--r-- | source/qFix/qfixSonar.h | 107 |
13 files changed, 2524 insertions, 0 deletions
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 <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
+
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<<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
+
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<<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
+
diff --git a/source/qFix/qfixI2C.h b/source/qFix/qfixI2C.h new file mode 100644 index 0000000..2597dcc --- /dev/null +++ b/source/qFix/qfixI2C.h @@ -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
+
diff --git a/source/qFix/qfixI2CDefs.h b/source/qFix/qfixI2CDefs.h new file mode 100644 index 0000000..5caa55c --- /dev/null +++ b/source/qFix/qfixI2CDefs.h @@ -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
+
diff --git a/source/qFix/qfixI2CMaster.h b/source/qFix/qfixI2CMaster.h new file mode 100644 index 0000000..63badd5 --- /dev/null +++ b/source/qFix/qfixI2CMaster.h @@ -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
+
diff --git a/source/qFix/qfixI2CSlave.h b/source/qFix/qfixI2CSlave.h new file mode 100644 index 0000000..30151d3 --- /dev/null +++ b/source/qFix/qfixI2CSlave.h @@ -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
+
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<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
+
+
+
diff --git a/source/qFix/qfixMega128Board.h b/source/qFix/qfixMega128Board.h new file mode 100644 index 0000000..0a14e00 --- /dev/null +++ b/source/qFix/qfixMega128Board.h @@ -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
+
diff --git a/source/qFix/qfixServoBoard4.h b/source/qFix/qfixServoBoard4.h new file mode 100644 index 0000000..baaa2f9 --- /dev/null +++ b/source/qFix/qfixServoBoard4.h @@ -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
+
+
+
diff --git a/source/qFix/qfixSlaveBoard.h b/source/qFix/qfixSlaveBoard.h new file mode 100644 index 0000000..ad781de --- /dev/null +++ b/source/qFix/qfixSlaveBoard.h @@ -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
+
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<<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
+
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 |