/* 
Pico code v0.1

Useage:
Drives the pico robot in a simple obstacle avoidance routine.
Mild immunity to ambient light conditions.

License:

Copyright (c) 2007 Zac Wheeler <zac@poor-robot.com>

Permission is hereby granted, free of charge, to any person
obtaining a copy of this software and associated documentation
files (the "Software"), to deal in the Software without
restriction, including without limitation the rights to use, copy,
modify, merge, publish, distribute, sublicense, and/or sell copies
of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be
included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.

*/

#include <avr/io.h>
#include <avr/wdt.h>
#include <avr/interrupt.h>
#include <util/delay.h>

// these control the visible LED
#define PORT_LED PORTA
#define DDR_LED DDRA
#define LED (1<<4)

#define _led_on() PORT_LED |= LED; DDR_LED |= LED
#define _led_off() PORT_LED &= ~LED; DDR_LED |= LED
#define _led_flip() PORT_LED ^= LED; DDR_LED |= LED

// these control the motors
#define PORT_MOTORS PORTA
#define DDR_MOTORS DDRA
#define PIN_MOTORS PINA
#define MOTOR_IN1 (1<<0)
#define MOTOR_IN2 (1<<1)
#define MOTOR_IN3 (1<<2)
#define MOTOR_IN4 (1<<3)

// these just setup the DDRs
#define _init_left_motor() \
  PORT_MOTORS &= ~(MOTOR_IN1|MOTOR_IN2); \
  DDR_MOTORS |= MOTOR_IN1|MOTOR_IN2
#define _init_right_motor() \
  PORT_MOTORS &= ~(MOTOR_IN3|MOTOR_IN4); \
  DDR_MOTORS |= MOTOR_IN3|MOTOR_IN4

// these set the IO for the 3901 to control motor direction
#define _left_motor_forward() \
  PORT_MOTORS |= MOTOR_IN1; \
  PORT_MOTORS &= ~MOTOR_IN2
#define _left_motor_backward() \
  PORT_MOTORS |= MOTOR_IN2; \
  PORT_MOTORS &= ~MOTOR_IN1
#define _left_motor_stop() \
  PORT_MOTORS |= MOTOR_IN1|MOTOR_IN2
#define _left_motor_roll() \
  PORT_MOTORS &= ~(MOTOR_IN1|MOTOR_IN2)

#define _right_motor_forward() \
  PORT_MOTORS |= MOTOR_IN3; \
  PORT_MOTORS &= ~MOTOR_IN4
#define _right_motor_backward() \
  PORT_MOTORS |= MOTOR_IN4; \
  PORT_MOTORS &= ~MOTOR_IN3
#define _right_motor_stop() \
  PORT_MOTORS |= MOTOR_IN3|MOTOR_IN4
#define _right_motor_roll() \
  PORT_MOTORS &= ~(MOTOR_IN3|MOTOR_IN4)

// these are the different motor states
#define BACKWARD MOTOR_IN1
#define FORWARD MOTOR_IN2
#define STOPPED 0xFF

// these are the two motors
#define LEFT 1
#define RIGHT 2

// reflective sensors
#define PORT_SENSORS PORTB
#define DDR_SENSORS DDRB
#define PIN_SENSORS PINB
#define LEFT_LED (1<<2)

#define _left_sensor_on() \
  DDR_SENSORS |= LEFT_LED; \
  PORT_SENSORS |= LEFT_LED
#define _left_sensor_off() \
  PORT_SENSORS  &= ~ LEFT_LED

volatile uint8_t g_left_motor_state;
volatile uint8_t g_right_motor_state;

// return the adc value on a given pin
uint16_t get_adc_val(uint8_t pin){
  uint16_t result;
  DIDR0 |= ADC7D;
  ADMUX = 0x07 & pin;
  ADCSRA = (1<<ADEN)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADSC);
  while(ADCSRA & (1<<ADSC));
  result = ADCW;
  ADCSRA = 0;
  return result;
}

// physically changes motor direction and assigns state variables
void set_motor_state(uint8_t motor, uint8_t state) {
  if(motor == LEFT) { 
    if(state == FORWARD) { 
      _left_motor_forward(); 
      g_left_motor_state = FORWARD; 
    } else if(state==BACKWARD) { 
      _left_motor_backward(); 
      g_left_motor_state = BACKWARD; 
    } else { 
      _left_motor_roll(); 
      g_left_motor_state = STOPPED; 
    }
  }
  else if(motor == RIGHT) { 
    if(state == FORWARD) { 
      _right_motor_forward(); 
      g_right_motor_state = FORWARD; 
    } else if(state==BACKWARD) { 
      _right_motor_backward(); 
      g_right_motor_state = BACKWARD; 
    } else { 
      _right_motor_roll(); 
      g_right_motor_state = STOPPED; 
    }
  }
}

// don't update the motor state to STOPPED 
// left motor PWM ISR - off state
ISR(TIM0_COMPA_vect){ _left_motor_roll(); }

// right motor PWM ISR - off state
ISR(TIM0_COMPB_vect){ _right_motor_roll(); }

// reset the motors to running
ISR(TIM0_OVF_vect)
{
  set_motor_state(LEFT, g_left_motor_state);
  set_motor_state(RIGHT, g_right_motor_state);
}

// this sets up timer0 for indirect PWM
// had to use indirect PWM because the IO layout doesn't allow me
// to use the hardware pins that would normally drive it
void init_timer0(void){
  TCCR0B = (1<<CS01);
  TCCR0A = (1<<WGM01)|(1<<WGM00);
  TIMSK0 = (1<<OCIE0A)|(1<<OCIE0B)|(1<<TOIE0);
  OCR0B = OCR0A = 127;
  TCNT0 = 0;
}

// just sets up the IO for motor control
void init_motors(void){
  _init_left_motor();
  _init_right_motor();
}

// sets the motor states
void set_motors(uint8_t left_motor, uint8_t right_motor){
  if(g_left_motor_state != left_motor)
    set_motor_state(LEFT, left_motor);
  if(g_right_motor_state != right_motor) 
    set_motor_state(RIGHT, right_motor);
}

uint8_t get_speed(uint8_t which_speed){
  if(which_speed == LEFT) { return OCR0A; }
  return OCR0B;
}

// sets the motor speeds - value is the on-time out of 255 possible
void set_speed(uint8_t speed_left, uint8_t speed_right) {
  if(get_speed(LEFT) != speed_left){ OCR0A = speed_left; }
  if(get_speed(RIGHT) != speed_right) { OCR0B = speed_right; }
}


void delay_ms(uint16_t ms) { while(ms--) { _delay_ms(1); } }
void delay_s(uint8_t s) { 
  while(s--) { 
    delay_ms(950); 
    _led_on(); 
    delay_ms(50); 
    _led_off();
  } 
}

// returns the ADC value on the sensor output when the LED is on
// the lower the value the less light is being reflected back

uint16_t sensor_reading(void) {
  uint16_t reading;
  _left_sensor_on();
  delay_ms(1);
  reading = get_adc_val(7);
  _left_sensor_off();
  delay_ms(1);
  reading = get_adc_val(7) - reading;
  return reading;
}

#define FORWARD_SPEED 38
#define TURN_SPEED 46
#define BACKWARD_SPEED 46

// dir not implemented
void backup_and_turn(uint8_t dir){
  set_speed(BACKWARD_SPEED, BACKWARD_SPEED);
  set_motors(BACKWARD, BACKWARD);
  delay_ms(300);
  set_speed(TURN_SPEED, TURN_SPEED);
  set_motors(BACKWARD, FORWARD);
  delay_ms(300);
}

void go_forward(void){
  set_speed(FORWARD_SPEED, FORWARD_SPEED);
  set_motors(FORWARD, FORWARD);
}

int main(void)
{
  cli();
  wdt_enable(WDTO_8S);
  init_motors();
  set_motors(STOPPED, STOPPED);
  init_timer0();
  _led_on();
  delay_ms(1000);
  _led_off();
  delay_ms(1000);
  sei();
  
  while(1){
    if (sensor_reading() > 50) { _led_off(); go_forward(); }
    else { _led_on(); backup_and_turn(0); }
    wdt_reset();
  }
 }