CSE 466 Final Project - Accelerometer Driven Car by Kelvin Lau and Mike Quinn
Home
| Accelerometer Use | Servo
Use | Building Car | Problems
Faced |
#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;
}
}