// control / network / logic init
static void init_ctl(void) {
// disable interrupts
cpu_irq_disable();
// intialize the event queue
init_events();
print_dbg("\r\n init_events");
// intialize encoders
print_dbg("\r\n init_encoders");
init_encoders();
// send ADC config
print_dbg("\r\n init_adc");
init_adc();
// start timers
print_dbg("\r\n init_sys_timers");
init_sys_timers();
// init_app_timers();
// enable interrupts
cpu_irq_enable();
}
void init_world()
{
init_encoders();
init_state();
init_goals_buffer();
init_captors();
}
int main()
{
sei();
char *rotat_cnt_str;
char *trans_cnt_str;
uint16_t rotat_cnt = 0;
uint16_t trans_cnt = 0;
initialize_LCD_driver();
init_encoders();
clear_rotat_encoder_cnt();
//Read the current rotational encoder count
while(rotat_cnt < ROTAT_MAX)
{
rotat_cnt = get_rotat_encoder_cnt();
}
lcd_erase();
lcd_puts("RE_DONE");
clear_trans_encoder_cnt();
//Read the current translational encoder count
while(trans_cnt < TRANS_MAX)
{
trans_cnt = get_trans_encoder_cnt();
}
lcd_erase();
lcd_puts("TE_DONE");
stop_encoders();
return 0;
}
int main(void)
{
/*
Получение адреса и типа устройства
*/
getDeviceAddrType(&devAddr, &devType);
initUSART(51);
/*
Настройка силовых выходов
*/
COM1_3_DIR |= _BV(COM1) | _BV(COM2) | _BV(COM3);
COM4_DIR |= _BV(COM4);
/*
Настройка в зависимости
от типа устройства
*/
ct = 0;
switch (devType)
{
case 0x01:
{
// КХЧ
// Настроить выходы управления акселератором
ACCEL_DIR |= _BV(ACCEL_EN) | _BV(ACCEL_DWN) | _BV(ACCEL_CLK);
changeAccelPosition(0xff,true);
/*************** дальномеры ***************/
// дальномеры: выходы триггеров 1, 2 и 3
RANGE_ECHO_TRIG_1_3_DIR |= _BV(RANGE_TRIG1) | _BV(RANGE_TRIG2) | _BV(RANGE_TRIG3);
// дальномеры: выход 4
RANGE_TRIG_4_DIR |= _BV(RANGE_TRIG4);
// дальномеры: вход echo без подтяжки
RANGE_ECHO_TRIG_1_3_DIR &= ~_BV(RANGE_ECHO);
GICR |= _BV(INT2); //Разрешить прерывание INT2 (PB2) (echo дальномера)
MCUCSR |= _BV(ISC2); //INT2 дергается по переднему фронту
/**************** Энкодеры ****************/
init_encoders();
// PC1, PC2 - INT0, Задние колеса
// PC0, PD6 - INT1, Передние колеса
DDRD &= ~_BV(PD6);
PORTD |= _BV(PD6);
DDRC &= ~_BV(PC2) & ~_BV(PC1) & ~_BV(PC0);
PORTC |= _BV(PC2) | _BV(PC1) | _BV(PC0);
GICR |= _BV(INT0) | _BV(INT1); //Разрешить прерывание INT0 (pd2) и INT1 (PD3)
MCUCR |= _BV(ISC01) | _BV(ISC11); //INT0 и INT1 дергаются по спадающему фронту
break;
}
case 0x02:
{
// КРУ
LIM_DIR &= ~_BV(LIM_LEFT) & ~_BV(LIM_RIGHT);
LIM_PORT |= _BV(LIM_LEFT) | _BV(LIM_RIGHT);
initKRUVars();
KRU_encoderInit();
break;
}
case 0x03:
{
// КСП
break;
}
case 0x04:
{
// КЭН
break;
}
}
/*
Инициализация главного таймера
*/
initMainTimer();
sei();
while(1)
{
//TODO:: Please write your application code
}
}
//Main Function
int main()
{
init_devices();
init_encoders();
lcd_set_4bit();
lcd_init();
int value=0;
forward();
velocity(130,130);
lcd_print(2,1,130,3);
lcd_print(2,5,130,3);
lcd_print(2,9,pathindex,2);
lcd_print(2,13,dirn,3);
while(1)
{
read_sensor();
follow();
if(isPlus())
{
read_sensor();
value = path[pathindex++];
// Code inserted for calculation of actual location wrt initial starting point ,
// It will consider direction also.
if (value == F)
{
// Move the bot forward, for location only , No movement on ground.
move_bot(FR);
}
else if (value == L)
{
// Move the bot left , for location only , No movement on ground.
move_bot(LT);
}
else if (value == R)
{
// Move the bot right, for location only , No movement on ground.
move_bot(RT);
}
else if (value == M)
{
// To stop the Bot and then break out
stop();
break;
}
orient(value);
/* lcd_print(2,9,pathindex,2);
lcd_print(2,13,dirn,3);
lcd_print(1,13,turnL,1);
lcd_print(1,15,turnR,1);
*/
}
if(turnL == 1)
{/*
lcd_print(1,13,turnL,1);
forward_mm(20);
stop();
velocity(180,180);
left_degrees(95);
//_delay_ms(120);
read_sensor();
// while(Left_white_line <0x40)
// {
// read_sensor();
// left();
// }
stop();
forward();
velocity(180,180);
turnL = 0;
*/
back_mm(50);
//stop();
//velocity(130,130);
stop();
left_degrees(50);
rotate_left_slowly();
forward();
velocity(130,130);
turnL = 0;
}
if(turnR == 1)
{
/*
lcd_print(1,15,turnR,1);
forward_mm(20);
stop();
velocity(180,180);
right_degrees(95);
//_delay_ms(200);
read_sensor();
// while(Right_white_line <0x30)
// {
// read_sensor();
// right();
// }
stop();
forward();
//follow();
velocity(180,180);
turnR = 0;
*/
back_mm(50);
//stop();
//velocity(130,130);
stop();
right_degrees(50);
rotate_right_slowly();
forward();
velocity(130,130);
turnR = 0;
}
}
// Three Beeps for Interval
buzzer_on();
_delay_ms(100);
buzzer_off();
_delay_ms(100);
buzzer_on();
_delay_ms(100);
buzzer_off();
_delay_ms(100);
buzzer_on();
_delay_ms(100);
buzzer_off();
_delay_ms(100);
//code to head-back to starting position , i.e. Origin
return_path_counter = reach_origin();
forward();
velocity(130,130);
int counter = 0;
int intermediate_value = 0;
while(counter < return_path_counter)
{
read_sensor();
follow();
if(isPlus())
{
read_sensor();
value = path_to_origin[counter];
counter++;
// Code inserted for calculation of actual location wrt initial starting point ,
// It will consider direction also.
if (intermediate_value == FR)
{
// Move the bot forward, for location only , No movement on ground.
value = F;
}
else if (value == LT)
{
// Move the bot left , for location only , No movement on ground.
value = L;
}
else if (value == RT)
{
// Move the bot right, for location only , No movement on ground.
value = R;
}
else if (value == ST)
{
value = M;
// specially inserted as break will not allow the bot to stop using "orient(value)".
orient(value);
break;
}
orient(value);
}
if(turnL == 1)
{
back_mm(50);
//stop();
//velocity(130,130);
stop();
left_degrees(50);
rotate_left_slowly();
forward();
velocity(130,130);
turnL = 0;
}
if(turnR == 1)
{
back_mm(50);
//stop();
//velocity(130,130);
stop();
right_degrees(50);
rotate_right_slowly();
forward();
velocity(130,130);
turnR = 0;
}
}
// Three beeps for Finish
buzzer_on();
_delay_ms(100);
buzzer_off();
_delay_ms(100);
buzzer_on();
_delay_ms(100);
buzzer_off();
_delay_ms(100);
buzzer_on();
_delay_ms(100);
buzzer_off();
_delay_ms(100);
}
/* MotorControl running in independent cog */
void motorControl(void *par){
float integral_error = 0.0; // Integral error between servos
float left_error = 0.0, right_error = 0.0; // Proportional velocity error values
int left_velClicks = 0, right_velClicks = 0; // Current Left & Right velocity (in clicks)
int left_last = 0, right_last = 0; // Previous Left & Right velocity (in clicks)
int angleDiff = 0; // Diff between current & desired heading
int turnSpeed = 0; // Speed robot should be turning at
init_encoders(); // Set up wheel encoders.
compass_init(0); // Initialize compass on bus.
while(1){
left_velClicks = get_velClicks(0); // Get current left velocity (in clicks)
right_velClicks = get_velClicks(1); // Get current right velocity (in clicks)
switch (mFunc){ // Based on desired motor function
case FORWARD: // Move robot Forward
case BACKWARD: // or Backward.
// integral_error = K_INT * // Integrate Left & Right velocity
// integrate(left_vel, right_vel, des_bias_clicks); // also introduce possible bias.
left_error = K_PRO *
(des_vel_clicks - left_velClicks - integral_error); // Proportional speed adjustment of left servo
right_error = K_PRO *
(des_vel_clicks - right_velClicks + integral_error); // Proportional speed adjustment of right servo
alter_power(left_error, 0); // Alter servo speeds as necessary
alter_power(right_error, 1);
break;
case LEFT: // Rotate Left or
case RIGHT: // Right desired number of degrees.
curHeading = compass_smplHeading(); // Get current heading from compass
angleDiff = compass_diff(curHeading, desHeading); // Diff between cur & Desired heading
if (mFunc == LEFT){ // Rotate Left desired number of degrees.
while(angleDiff > 0){
turnSpeed = abs(angleDiff)/2; // Use half the turn angle as turn speed.
if(turnSpeed < 10) turnSpeed = 10; // Unless the turn angle is less than 10.
servo_set(WHEEL_L_PIN, 1500-turnSpeed); // Rotate Left wheel
servo_set(WHEEL_R_PIN, 1500-turnSpeed); // Rotate Right wheel
pause(8); // Make sure there is at least 8ms between compass readings.
curHeading = compass_smplHeading(); // Get current heading from compass
angleDiff = compass_diff(curHeading, desHeading); // Diff between Current & Desired heading
}
} else {
if ( mFunc == RIGHT){ // Rotate Right desired number of degrees.
while(angleDiff < 0){
turnSpeed = abs(angleDiff)/2; // Use half the turn angle as turn speed.
if(turnSpeed < 10) turnSpeed = 10; // Unless the turn angle is less than 10.
servo_set(WHEEL_L_PIN, 1500+turnSpeed); // Rotate Left wheel
servo_set(WHEEL_R_PIN, 1500+turnSpeed); // Rotate Right wheel
pause(8); // Make sure there is at least 8ms between compass readings.
curHeading = compass_smplHeading(); // Get current heading from compass
angleDiff = compass_diff(curHeading, desHeading); // Diff between Current & Desired heading
}
} else {
mFunc = STOP; // Rotation complete or invalid parameters.
servo_set(WHEEL_L_PIN, 1500); // Force Left servo to stop
servo_set(WHEEL_R_PIN, 1500); // Force Right servo to stop
}
}
break;
case STOP: // Stop servo motion immediately
default:
mFunc = STOP; // Set motor function to stop
servo_set(WHEEL_L_PIN, 1500); // Force Left servo to stop
servo_set(WHEEL_R_PIN, 1500); // Force Right servo to stop
integral = 0.0; // Reset Integral to zero
break;
}
pause(control_interval);
}
}