/* laser.c */ /* laser sensor management. {{{ * * Copyright (C) 2012 Florent Duchon * * APBTeam: * Web: http://apbteam.org/ * Email: team AT apbteam DOT org * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ #include #include #include "debug_avr.h" #include "laser.h" #include "servo.h" #include "codewheel.h" laser_s laser; /* This function initializes the laser pin input and associated interrupt */ void laser_init(void) { /* Init laser structiure */ laser_set_angle(0); /* Configure Input compare interrupts for Laser Interrupt*/ TCCR3B |= (1< CODEWHEEL_CPR - LASER_CONFIRMATION_OFFSET) { OCR3B = LASER_CONFIRMATION_OFFSET - (CODEWHEEL_CPR - value); } else { OCR3B = value + LASER_CONFIRMATION_OFFSET; } /* Enable interrupt */ TIMSK3 |= (1<= 360 - SERVO_ANGLE_POSITION_TOLERANCE)) && ((servo_get_state(SERVO_1) == SERVO_SCANNING_FAST_IN_PROGRESS) || (servo_get_state(SERVO_1) == SERVO_SCANNING_SLOW_IN_PROGRESS))) { calibration_set_laser_flag(SET); } /* If mire 2 is spotted */ else if(((laser_get_angle_degree() <= SERVO_2_ANGLE_POSITION + SERVO_ANGLE_POSITION_TOLERANCE) && (laser_get_angle_degree() >= SERVO_2_ANGLE_POSITION - SERVO_ANGLE_POSITION_TOLERANCE)) && ((servo_get_state(SERVO_2) == SERVO_SCANNING_FAST_IN_PROGRESS) || (servo_get_state(SERVO_2) == SERVO_SCANNING_SLOW_IN_PROGRESS))) { calibration_set_laser_flag(SET); } } } else { // TODO: Send angle } /* Disable the interrupt */ laser_inhibit_angle_confirmation(); } /* Laser IRQ vector */ ISR(TIMER3_CAPT_vect) { static uint16_t virtual_angle = 0; TLaser_edge_type current_edge; /* Check which kind of edge triggered the interrupt */ current_edge = laser_get_edge_type(); /* Could be a bounce so inhibit the latest angle confirmation */ laser_inhibit_angle_confirmation(); switch(current_edge) { /* First rising edge of a reflector */ case LASER_FIRST_RISING_EDGE: virtual_angle = ICR3; break; /* Common rising edge of a reflector */ case LASER_RISING_EDGE: /* Recompute the angle value */ virtual_angle = (virtual_angle + ICR3) / 2; break; /* Falling edge detected */ case LASER_FALLING_EDGE: /* Recompute the angle value */ virtual_angle = (virtual_angle + ICR3) / 2; /* UseI ICR3 for now*/ virtual_angle = ICR3; /* It's a falling edge so potentially current_angle could be a real one */ laser_set_angle(virtual_angle); /* Start virtual angle confirmation */ laser_engage_angle_confirmation(ICR3); break; default: break; } /* Invert the edge detection to catch next rising or falling edge */ laser_invert_IRQ_edge_trigger(); }