//------------------------------------------------------------------
// 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

