142 lines
3.5 KiB
C
Executable file
142 lines
3.5 KiB
C
Executable file
#include "robot.h"
|
|
|
|
//-----------------------------------------------------------------------------
|
|
Robot::Robot()
|
|
{
|
|
//Hardware
|
|
|
|
//Set pin 1-6 output, 0 and 7 input
|
|
DDRA = (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6);
|
|
PORTA = 0;
|
|
|
|
//All output
|
|
DDRB = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7);
|
|
PORTB = (1 << 1);
|
|
|
|
//All output except PC4/PC7 (mousesensor SDA)
|
|
DDRC = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 5) | (1 << 6);
|
|
PORTC = 0;
|
|
|
|
//All output except PD0+1(I2C) + 2+3(RS232)
|
|
DDRD = (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7);
|
|
PORTD = (1 << 0) | (1 << 1);//Activate pullup at PD0+1
|
|
|
|
//PE5 for input
|
|
DDRE = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 6) | (1 << 7);
|
|
PORTE = 0;
|
|
|
|
//All input with pullup
|
|
DDRF = 0;
|
|
PORTF = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7);
|
|
|
|
//All input
|
|
DDRG = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) | (1 << 4);
|
|
PORTG = (1 << 0) | (1 << 1);
|
|
|
|
// activate channel A+B on PWM1 at 8Bit
|
|
TCCR1A = (1<< COM1A1) | (1<< COM1B1) | (1<< WGM10);
|
|
TCCR1B = (1<<ICNC1) | (1<<CS11); // set clock/prescaler 1/8 -> enable counter
|
|
|
|
// activate Kanal A+B on PWM3 at 8Bit
|
|
TCCR3A = (1<< COM3A1) | (1<< COM3B1) | (1<< WGM10);
|
|
TCCR3B = (1<<ICNC3) | (1<<CS31); // set clock/prescaler 1/8 -> enable counter
|
|
|
|
//Activate interrupt
|
|
sei();
|
|
|
|
// Initialiate TWI
|
|
Init_TWI();
|
|
|
|
//Interface
|
|
for(uint8 i = IO_START; i < IO_END; i++)
|
|
{
|
|
modules[i] = NULL;
|
|
}
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
Robot::~Robot()
|
|
{
|
|
for(uint8 i = 0; i < IO_END; i++)
|
|
{
|
|
if(!modules[i]) continue;
|
|
|
|
delete modules[i];
|
|
modules[i] = NULL;
|
|
}
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
bool Robot::AddModule(IO_Module* newModule)
|
|
{
|
|
if(modules[newModule->GetId()])
|
|
return false;
|
|
|
|
newModule->SetParent(this);
|
|
|
|
modules[newModule->GetId()] = newModule;
|
|
|
|
return true;
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
bool Robot::RemoveModule(IO_Module* oldModule)
|
|
{
|
|
return RemoveModule(oldModule->GetId());
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
void Robot::Update()
|
|
{
|
|
GetModule<Command_Handler>(IO_COMMAND_HANDLER_MAIN)->Update();
|
|
|
|
GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN)->Update();
|
|
|
|
GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN)->Update();
|
|
|
|
GetModule<Obstacle_Tracker>(IO_OBSTACLE_TRACKER_MAIN)->Update();
|
|
|
|
//GetModule<Logic>(IO_LOGIC_MAIN)->Update();
|
|
|
|
GetModule<Navigator>(IO_NAVIGATOR_MAIN)->Update();
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
uint16 Robot::GetADCValue(uint8 channel)
|
|
{
|
|
if(channel > 7) return 0;
|
|
|
|
uint32 result = 0;
|
|
|
|
//Activate ADC and set division factor to 128
|
|
ADCSRA = (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);
|
|
|
|
//Set multiplexer channel
|
|
ADMUX = channel;
|
|
//Use internal referencevoltage (2,56 V for atmega32)
|
|
ADMUX |= (1 << REFS1) | (1 << REFS0);
|
|
|
|
//Initialise ADC and start dummyreadout
|
|
ADCSRA |= (1 << ADSC);
|
|
while(ADCSRA & (1 << ADSC));
|
|
|
|
//Get voltage three times and calculate average value
|
|
for(uint8 i = 0; i < 3; i++)
|
|
{
|
|
// Eine Wandlung
|
|
ADCSRA |= (1 << ADSC);
|
|
// Auf Ergebnis warten...
|
|
while(ADCSRA & (1 << ADSC));
|
|
|
|
result += ADCW;
|
|
}
|
|
|
|
//Disable ADC
|
|
ADCSRA &= ~(1 << ADEN);
|
|
|
|
result /= 3;
|
|
|
|
return (uint16)(result);
|
|
}
|
|
|
|
//--- EOF ---
|