#include <At89x55.h>

// GLOBAL DEFINES
// **********************************

// as measured. Used 1.2M Ohm resistor.
#define ACCELL_FREQUENCY 130.5

// about 20,434
#define TIMER_COUNT_PER_PERIOD ( (32000000 / 12) / ACCELL_FREQUENCY )

// DEFINES FOR MOTOR
//***********************************

// determined by oscilliscope
#define DUTYHIGH_AT_LEVEL_X .48
#define DUTYHIGH_AT_HIGH_X .60
#define DUTYHIGH_AT_LOW_X .35

// how much we should count while level and at extreme angles 
#define DUTYCOUNT_LOW_X (TIMER_COUNT_PER_PERIOD * DUTYHIGH_AT_LOW_X)
#define DUTYCOUNT_MID_X (TIMER_COUNT_PER_PERIOD * DUTYHIGH_AT_LEVEL_X)
#define DUTYCOUNT_HIGH_X (TIMER_COUNT_PER_PERIOD * DUTYHIGH_AT_HIGH_X)

// x = size of speeds array; x - 1 == MAX_SPEED_INDEX
#define MAX_SPEED_INDEX 13

// window where we don't move the motor; trying to stop
#define WINDOW 100
#define AVERAGESIZE 128
#define RAMP_UP_TIME 30


// DEFINES FOR STEERING CONTROL
// *****************************************

#define DUTYHIGH_AT_LEVEL_Y .46
#define DUTYHIGH_AT_HIGH_Y .60
#define DUTYHIGH_AT_LOW_Y .35

// how much we should count while level and at extreme angles
#define DUTYCOUNT_LOW_Y (TIMER_COUNT_PER_PERIOD * DUTYHIGH_AT_LOW_Y)
#define DUTYCOUNT_MID_Y (TIMER_COUNT_PER_PERIOD * DUTYHIGH_AT_LEVEL_Y)
#define DUTYCOUNT_HIGH_Y (TIMER_COUNT_PER_PERIOD * DUTYHIGH_AT_HIGH_Y)

// 20834 * 0.020 = 41.66 or 42
#define TWENTY_MIL_COUNT 417

#define STEER_DELAY_COUNT 10
#define MAX_ANGLE_INDEX 13

// compares with value in speed[speed_index]
unsigned int flip_bit_counter;

// controls when we actually change the current speed_index
unsigned int adjust_speed_counter;

// current and desired speed indices
short speed_index;
short desired_speed_index;

// 0 if DutyCounterSpeed < DUTYCOUNT_MID_X
// 1 if DutyCounterSpeed > DUTYCOUTN_MID
unsigned short current_direction;
unsigned short desired_direction; 

unsigned short DutyCounterSpeed;


// stopped flag
unsigned short stopped;

// array that holds the incremented speeds. speed[0] is slow speed[19] is fast
const unsigned short speed[14] = { 500, 350, 255, 180, 135, 111, 98, 80, 70, 63, 57, 52, 47, 44};


// every time count is .0479 millisecond
// all left at 1.25ms, neutral at 1.50ms, all right at 1.75ms
const unsigned char angle[14] = { 19, 21, 23, 25, 27, 29, 31, 33, 35, 37, 39, 41, 43, 45 };

unsigned long sum;
unsigned int average;
unsigned short DutyCounterAngle;
short angle_index;
short desired_angle_index;

unsigned int adjust_angle_counter;
int duration_counter;
unsigned int twenty_mil_counter;


// gets called when the carry bit is set on an 8 bit counter
// calculated at 10.4 kHz 
//[we are going to change this to 20.8 KHz so we have enough granularity to run the servo]
// we have plenty of clock cycles to deal with still. 2,666,666 to be exact.
void adjust_settings (void) interrupt 5 using 0 {

TF2 = 0; // reset interrupt flag


// counter for determining motor driver frequency
flip_bit_counter++; 

// actually do a bit flip if we need to
if( flip_bit_counter > speed[speed_index] ){

// only flip the bit if we are NOT in stopped mode 
if(!stopped) 
P1_2 = !P1_2; // flip motor driver output

flip_bit_counter = 0; 
}

// keeps track of twenty milliseconds
twenty_mil_counter++;
duration_counter--;

P1_7 = !P1_7;
if( twenty_mil_counter > TWENTY_MIL_COUNT ){
P1_5 = !P1_5;
twenty_mil_counter = 0;
duration_counter = angle[angle_index];
P1_4 = 1;
}

if( duration_counter < 0 ) {
P1_4 = 0;
P1_6 = !P1_6;
}

}


// gets called on every high to low transition on input (from accelarometer) on INT0
// 128.5 hz is the measured frequency this function gets called
void high_to_low_speed (void) interrupt 0 using 1 {

DutyCounterSpeed = 0;
adjust_speed_counter++; // when to adjust desired speed

// move two 8-bit counter to one 16-bit counter
DutyCounterSpeed = TH0;
DutyCounterSpeed = DutyCounterSpeed << 8;
DutyCounterSpeed += TL0;

// reset counter
TH0 = 0;
TL0 = 0;

// ADJUST THE SPEED OF THE MOTOR
// ****************************************

// time to change the desired speed of motor
if(adjust_speed_counter > RAMP_UP_TIME){

// Determine what speed we want to be at by calculated the desired
// index into our array that determines speed

// 1/130 = 19 / 2480
desired_speed_index = (DutyCounterSpeed - DUTYCOUNT_MID_X) / 130;
if(desired_speed_index < 0){ desired_speed_index = desired_speed_index * -1; }

// sanity check that we are in bounds
if(desired_speed_index > MAX_SPEED_INDEX) desired_speed_index = MAX_SPEED_INDEX;
if(desired_speed_index < 0) desired_speed_index = 0; 

// check what direction we want to be going
if(DutyCounterSpeed < DUTYCOUNT_MID_X) { 
desired_direction = 0;
}else{
desired_direction = 1;
}

// if we need to change directions then slow down
if( desired_direction != current_direction ){

speed_index--; // slow down

// if DutyCount within a small window then we are stopping (eventually stopped)
}else if( -WINDOW < (DutyCounterSpeed - DUTYCOUNT_MID_X) && (DutyCounterSpeed - DUTYCOUNT_MID_X) < WINDOW ){

speed_index--; // slow down

// currently going faster than we want
}else if( speed_index > desired_speed_index ){

speed_index--; // slow down

// currently going slower than we want
}else if( speed_index < desired_speed_index ){

speed_index++; // speed up
stopped = 0;
}

// do a sanity check on values
if( speed_index < 0 ) speed_index = 0;
if( speed_index > MAX_SPEED_INDEX ) speed_index = MAX_SPEED_INDEX;

// we are stopped and can change direction
if( speed_index == 0 ){
stopped = 1;
if(desired_direction == 1){
P1_3 = 1;
current_direction = 1;
}else{
P1_3 = 0;
current_direction = 0;
}
}

// reset counter for adjusting desired speed
adjust_speed_counter = 0;
}

}


// gets called on every high to low transition on input (from accelarometer) on INT1
// 128.5 hz is the measured frequency this function gets called
void high_to_low_angle (void) interrupt 2 using 2 {

// for testing
P0_5 = !P0_5; 

DutyCounterAngle = 0;
adjust_angle_counter++; // when to adjust desired speed
//P1_6 = !P1_6; 

// move two 8-bit counter to one 16-bit counter
DutyCounterAngle = TH1;
DutyCounterAngle = DutyCounterAngle << 8;
DutyCounterAngle += TL1;
P0_6 = !P0_6;
// reset counter
TH1 = 0;
TL1 = 0;

// let's keep a sum of 16 readings

sum -= average;
sum += DutyCounterAngle;
average = sum >> 5;
DutyCounterAngle = average;

//P1_7 = !P1_7;
if( adjust_angle_counter > STEER_DELAY_COUNT ){

//P1_5 = !P1_5;

// 1/365 = 14 / 5121
desired_angle_index = (DutyCounterAngle - DUTYCOUNT_LOW_Y) / 365;

// sanity check that we are in bounds
if(desired_angle_index > MAX_ANGLE_INDEX) desired_angle_index = MAX_ANGLE_INDEX;
if(desired_angle_index < 0) desired_angle_index = 0; 

if( angle_index == desired_angle_index ){
// do nothing
}else if( angle_index > desired_angle_index ){
angle_index--; 

}else if( angle_index < desired_angle_index ){
angle_index++; 
}

// do a sanity check on values
if( angle_index < 0 ) angle_index = 0;
if( angle_index > MAX_ANGLE_INDEX ) angle_index = MAX_ANGLE_INDEX;

adjust_angle_counter = 0;
}
}



void main(){

// initialization of global variables
adjust_speed_counter= 0; 
speed_index = 0;
stopped = 1;
current_direction = 1;

sum = 32 * DUTYCOUNT_MID_Y;
average = DUTYCOUNT_MID_Y;
twenty_mil_counter = 0;
adjust_angle_counter= 0;
duration_counter = 0;
angle_index = 5; // this is neutral

TMOD = 0x99; // timer 0 is enabled as a 16 bit timer and only counts when INT0 is high
// timer 1 is enabled as a 16 bit timer and only counts when INT1 is high

TCON = 0x55; // enable timers to run
// enable timers to interrupt on falling edge
// enable timers to reset the interrupt flab

INT0 = 1; // enable the input ports for reading
INT1 = 1;

IE = 0xA5; // global enable of interrupts
// enable external interrupt 0 and 1
// enable timer2 interrupt [not sure if this is implemented]

TL0 = 0x00; // reset the timers to 0x0000
TH0 = 0x00;
TL1 = 0x00;
TH1 = 0x00;
TL2 = 0x00;
TH2 = 0x00;

P1 = 1;
P0 = 0xFF;

// setup timer 2 so that we have an 8-bit counter w/ interrupt
T2MOD = 0x00;
T2CON = 0x04; // start timer 2
// internal timer
// auto-reload on overflow

RCAP2L = 0x80; // timer 2 will be reset with these values
RCAP2H = 0xFF;


while(1){
// go IDLE
PCON = 0x01;
}
}