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