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/codewheel.c | 1 + digital/beacon/src/motor.c | 65 +++++++++++++++++++++++++++++++++++++++--- digital/beacon/src/motor.h | 25 ++++++++++++++++ 3 files changed, 87 insertions(+), 4 deletions(-) (limited to 'digital') diff --git a/digital/beacon/src/codewheel.c b/digital/beacon/src/codewheel.c index d5743937..8b8e6ed8 100644 --- a/digital/beacon/src/codewheel.c +++ b/digital/beacon/src/codewheel.c @@ -114,6 +114,7 @@ ISR(TIMER3_COMPA_vect) else { OCR3A = CODEWHEEL_CPR; + motor_control_speed(codewheel.time); codewheel.time = 0; } laser_reset_angle_id(); 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) { diff --git a/digital/beacon/src/motor.h b/digital/beacon/src/motor.h index c0b2c4f0..df947c9b 100644 --- a/digital/beacon/src/motor.h +++ b/digital/beacon/src/motor.h @@ -26,6 +26,16 @@ #ifndef _MOTOR_H #define _MOTOR_H +#define MOTOR_TARGET_SPEED_INIT 110 +#define MOTOR_SPEED_STOP 0 +#define MOTOR_SPEED_MIN 90 +#define MOTOR_SPEED_MAX 140 + +typedef struct +{ + uint16_t target_speed; +} motor_s; + typedef enum { MOTOR_STOPPED, @@ -41,10 +51,25 @@ void motor_start(void); /* This function stops the motor rotation */ void motor_stop(void); +/* This function sets the motor speed */ +void motor_set_speed(uint8_t value); + +/* This function returns the motor speed in raw format */ +uint8_t motor_get_speed_raw(); + /* This function returns the motor state */ TMotor_state motor_get_state(void); /* This function starts or stops the motor according to the current state */ void motor_start_stop_control(void); +/* This function sets the target speed */ +void motor_set_target_speed(uint8_t value); + +/* This function returns the target speed */ +uint8_t motor_get_target_speed(); + +/* This function control the motor speed accroding to target speed requested */ +void motor_control_speed(uint16_t time); + #endif -- cgit v1.2.3