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