//345 project //Team #2
#include <avr/io.h> /* ATMEGA32 I/O register defines */
#include <avr/signal.h>
#include <avr/interrupt.h>
#include "sio.c"
// Deadband Limits
#define c_kinetic_pos 0x95 // Left Motor
#define c_kinetic_neg 0xA4
#define c_static_pos 0xd0
#define c_static_neg 0xe6
#define c_kinetic_pos2 0x65 // Right Motor
#define c_kinetic_neg2 0x90
#define c_static_pos2 0xc0
#define c_static_neg2 0xd6
#define c_max 255
#define c_min 255 /* make the value positive */
#define T 4
#define Kp 5
#define Ki 5
#define Kd 5
int db_correct = 1; // deadband correction is on by default int Angle_int;
int Pdesired = 0x88; // Desired vertical position of the pot
int sensor1; // Change in voltage at each sensor due to a change in resistance int sensor2;
void delay(int ticks);
void IO_update(void);
int AD_read();
/**************************Integration**************************/
int integrateL(int e1) {
e_sum1 += e1 * T;
return e_sum1;
}
int integrateR(int e2) {
e_sum2 += e2 *T;
return e_sum2;
}
int diffR(int errorR) {
int DR;
DR = errorR - VinitR; // Differentiate pot position VinitR = errorR;
return DR;
}
int diffL(int errorL) {
int DL;
DL = errorL - VinitL; // Differentiate pot position VinitL = errorL;
return DL;
}
#define CLK_ms 10 // set the updates for every 10ms, same as CLK_ms /***********************Controller Function********************/
//int Cd = 0; // Initialize desired position int controllerL(int Cd, int Cf)
{
int Ce; // Position error
int Cw; // Error with gain compensation Ce = Cd - Cf;
Cw = Kp * Ce + (Ki * integrateL(Ce) * CLK_ms/ 1000) + Kd*diffL(Ce) *(1000/CLK_ms);
return Cw;
}
int controllerR(int Cd, int Cf) {
int Ce;
int Cw;
Ce = Cd - Cf;
Cw = Kp * Ce + (Ki * integrateR(Ce) * CLK_ms/ 1000)+ Kd*diffR(Ce) * (1000/CLK_ms);
return Cw;
}
// Read the pot
int pot_feedback() {
ADMUX = 0x00; // set the input to channel 1 (PA0)
ADCSRA = 0xC0; // turn on the ADC and set it to read once while((ADCSRA & _BV(ADSC)) != 0x00){}
Angle_int = AD_read();
return Angle_int;
ADCSRA =0;
}
/*******************************Clock Setup********************************/
unsigned int CNT_timer1; // the delay time
volatile unsigned int CLK_ticks = 0; // the current number of ms volatile unsigned int CLK_seconds = 0; // the current number of seconds SIGNAL(SIG_OVERFLOW0){
// The interrupt calls this function (no change in function) CLK_ticks += CLK_ms;
if(CLK_ticks >= 1){ // The number of interrupts between output changes CLK_ticks = CLK_ticks - 1;
CLK_seconds++;
IO_update();
}
TCNT0 = CNT_timer1;
}
void CLK_setup(){ // Start the interrupt service routine //no change in function
TCCR0 = (0<<FOC0) | (0<<WGM01)| (0<<WGM00) | (0<<COM00)| (0<<COM01) | (1<<CS02)
| (0<<CS01) | (1<<CS00);
// use CLK/1024 prescale value
// disable PWM and Compare Output Modes CNT_timer1 = 0xFFFF - CLK_ms * 8; // 8 = 1ms, 80 = 10ms
TCNT0 = CNT_timer1; // start at the right point TIFR |= (1<<TOV0);
TIFR &= ~(0<<OCF0);
TIMSK |= (1<<TOIE0);
TIMSK &= ~(0<<OCIE0);
sei(); // enable interrupts flag
}
/************************ end clock setup************************************/
/********************************Deadbands***********************************/
{
int c_pos1;
int c_neg1;
int c_adjustedL;
if(motorL > 0) {
c_pos1 = c_kinetic_pos;
c_neg1 = c_kinetic_neg;
} else {
c_pos1 = c_static_pos;
c_neg1 = c_static_neg;
}
if(c_wanted == 0) /* turn off the output */
{
c_adjustedL = 0;
}
else if(c_wanted > 0) /* a positive output */
{
c_adjustedL = c_pos1 + (unsigned)(c_max - c_pos1) * c_wanted / c_max;
if(c_adjustedL > c_max) c_adjustedL = c_max;
}
else /* the output must be negative */
{
c_adjustedL = -c_neg1 - (unsigned)(c_min - c_neg1) * -c_wanted / c_min;
if(c_adjustedL < -c_min) c_adjustedL = -c_min;
}
return c_adjustedL;
}
int deadbandR(int c_wanted) /* call this routine when updating */
{
int c_pos2;
int c_neg2;
int c_adjustedR;
if(motorR > 0) {
c_pos2 = c_kinetic_pos2;
c_neg2 = c_kinetic_neg2;
} else {
c_pos2 = c_static_pos2;
c_neg2 = c_static_neg2;
}
if(c_wanted == 0) /* turn off the output */
{
c_adjustedR = 0;
}
else if(c_wanted > 0) /* a positive output */
c_adjustedR = c_pos2 + (unsigned)(c_max - c_pos2) * c_wanted / c_max;
if(c_adjustedR > c_max) c_adjustedR = c_max;
}
else /* the output must be negative */
{
c_adjustedR = -c_neg2 - (unsigned)(c_min - c_neg2) * -c_wanted / c_min;
if(c_adjustedR < c_min) c_adjustedR = -c_min;
}
return c_adjustedR;
}
/******************************End of Deadband********************************/
int AD_read() {
return (ADCW);
}
/*******************************PWM Stuff*************************************/
void PWM_init() /* call this routine once when the program starts */
{
DDRD |= (1 << PD5) | (1 << PD4); /* set PWM outputs */
DDRC |= (1 << PC0) | (1 << PC1) | (1 << PC2) | (1 << PC3);
/* set motor direction outputs on port C*/
/*using OCR1*/
TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);
// turn on both PWM outputs on counter 1 TCCR1B = _BV(CS11) ; // set the internal clock to 1/8 clock }
void PWM_updateL(int value) /* to update the PWM output for motor1*/
{
if(value > 255) value = 255;
if(value < 0) value = 0;
OCR1A = value; // duty cycle motor 1 }
void PWM_updateR(int value2) /* to update the PWM output for motor 2*/
{
if(value2 > 255) value2 = 255;
if(value2 < 0) value2 = 0;
OCR1B = value2; // duty cycle
/******************************End of PWM Stuff*******************************/
/*******************************Voltage Output********************************/
int v_outputL(int v_adjustedL) /* call from the interrupt loop */
{
int RefSignal; // the value to be returned
if(v_adjustedL >= 0)
{ /* set the direction bits to CW on, CCW off */
PORTC = (PINC & 0xFC) | 0x02; /* bit 1 on, 0 off */
if(v_adjustedL > 255) /* clip output over maximum */
{
RefSignal = 255;
} else {
RefSignal = v_adjustedL;
} }
else { /* need to reverse output sign */
/* set the direction bits to CW off, CCW on */
PORTC = (PINC & 0xFC) | 0x01; /* bit 0 on, 1 off */
if(v_adjustedL < -255) /* clip output below minimum */
{
RefSignal = 255;
/* the output must be negative */
} else {
RefSignal = -v_adjustedL; /* flip sign */
} }
return RefSignal;
} //adjusting for negative voltages
int v_outputR(int v_adjustedR) /* call from the interrupt loop */
{
int RefSignal;
if(v_adjustedR >= 0)
{ /* set the direction bits to CW on, CCW off */
PORTC = (PINC & 0xF3) | 0x04; /* bit 3 on, bit 4 off*/
if(v_adjustedR > 255) /* clip output over maximum */
{
RefSignal = 255;
} else {
RefSignal = v_adjustedR;
} }
else /* need to reverse output sign */
{ /* set the direction bits to CW off, CCW on */
PORTC = (PINC & 0xF3) | 0x08; /* bit 3 off, bit 4 on */
{
RefSignal = 255;
} else {
RefSignal = -v_adjustedR; /* flip sign */
} }
return RefSignal;
} //adjusting for negative voltages
/*******************************End Of Voltage Output*******************************/
void IO_setup() {
PWM_init();
CLK_setup();
ADCSRA=(ADCSRA|0x80); //turn ADC on
}
void delay(int ticks) // ticks are approximately 1ms {
volatile int i, j;
for(i = 0; i < ticks; i++) {
for(j = 0; j < 1000; j++){}
}
} //this function sucks and is never called
/*******************************Photo Sensors********************************/
int sensor1_func() /* reads sensor1 (right sensor)*/
{
ADMUX = 0x01; // set the input to channel PA0
ADCSRA = 0xC0; // turn on the ADC and set it to read once while((ADCSRA & _BV(ADSC)) != 0){}
sensor1 = (AD_read());
return sensor1;
ADCSRA = 0;
}
int sensor2_func() /* reads sensor2 (middle sensor)*/
{
ADMUX = 0x03; // set the input to channel 1 PA3
ADCSRA = 0xC0; // turn on the ADC and set it to read once while((ADCSRA & _BV(ADSC)) != 0x00){}
sensor2 = (AD_read());
ADCSRA = 0;
}
int sensor3_func() // reads sensor3 (left sensor) {
ADMUX = 0x05; // set the input to channel 1 PA5
ADCSRA = 0xC0; // turn on the ADC and set it to read once while((ADCSRA & _BV(ADSC)) != 0x00){}
sensor3 = (AD_read());
return sensor3;
ADCSRA = 0;
}
void sensorcompare() // Compare the sensor values {
if(sensor2 <= 0x78) {
motorL = 50;
motorR = 50;
if(sensor1 > 0x19e && sensor2 < 0x02b8) {
motorL += 0x04;
motorR -= 0x04;
}
else if(sensor2 < 0x02b8 && sensor3 > 0x02ce) {
motorL -= 0x04;
motorR += 0x04;
} else {
motorL = motorL;
motorR = motorR;
}
/*****************************End of Photo Sensors****************************/
void IO_update() // This routine will run once per interrupt for updates
sensor1_func();
sensor2_func();
sensor3_func();
sensorcompare();
PWM_updateL(v_outputL(deadbandL(motorL)));
PWM_updateR(v_outputR(deadbandR(motorR)));
pot_feedback();
int Pactual = Angle_int;
PWM_updateL(v_outputL(deadbandL(controllerL(Pdesired, Pactual))));
PWM_updateR(v_outputR(deadbandR(controllerR(Pdesired, Pactual))));
}
int main() {
sio_init();
IO_setup();
sio_cleanup();
return 0;
}