From 403d9c808077b7e581ed144a6b85615c9006613a Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Thu, 17 May 2012 01:59:32 +0200 Subject: digital/beacon: add motor control speed --- digital/beacon/src/motor.c | 65 +++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 61 insertions(+), 4 deletions(-) (limited to 'digital/beacon/src/motor.c') diff --git a/digital/beacon/src/motor.c b/digital/beacon/src/motor.c index 8105a865..55da6a21 100644 --- a/digital/beacon/src/motor.c +++ b/digital/beacon/src/motor.c @@ -23,16 +23,19 @@ * * }}} */ +#include "debug_avr.h" #include "motor.h" #include "codewheel.h" +motor_s motor; + /* This function initializes the motor control output */ void motor_init(void) { /* Select ouptut */ DDRB |= (1<= MOTOR_SPEED_MAX) + OCR0A = MOTOR_SPEED_MAX; + else if(value <= MOTOR_SPEED_STOP) + OCR0A = MOTOR_SPEED_STOP; + else + OCR0A = value; +} + +/* This function returns the motor speed in raw format */ +uint8_t motor_get_speed_raw() +{ + return OCR0A; +} + /* This function returns the motor state */ TMotor_state motor_get_state(void) { - if(OCR0A != 0) + if(OCR0A > MOTOR_SPEED_MIN) return MOTOR_IN_ROTATION; else return MOTOR_STOPPED; @@ -74,11 +97,45 @@ TMotor_state motor_get_state(void) void motor_start_stop_control(void) { if(motor_get_state() == MOTOR_IN_ROTATION) + { motor_stop(); + } else + { motor_start(); + } +} + +/* This function sets the target speed */ +void motor_set_target_speed(uint8_t value) +{ + motor.target_speed = value; +} + +/* This function returns the target speed */ +uint8_t motor_get_target_speed() +{ + return motor.target_speed; } +/* This function control the motor speed accroding to target speed requested */ +void motor_control_speed(uint16_t time) +{ + int16_t diff = 0; + int16_t correction = 0; + + diff = motor_get_target_speed() - time; + + if(diff > 15) + correction = motor_get_speed_raw() - 5; + else if (diff < -15) + correction = motor_get_speed_raw() + 5; + else + correction = motor_get_speed_raw(); + + motor_set_speed(correction); + +} ISR(TIMER0_COMPA_vect) { -- cgit v1.2.3