summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/beacon/src/codewheel.c1
-rw-r--r--digital/beacon/src/motor.c65
-rw-r--r--digital/beacon/src/motor.h25
3 files changed, 87 insertions, 4 deletions
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<<PB7);
- OCR0A = 0;
+ OCR0A = MOTOR_SPEED_MIN;
/* Fast PWM 10bits with TOP=0x03FF */
TCCR0A |= (1<<WGM01)|(1<<WGM00);
@@ -43,6 +46,7 @@ void motor_init(void)
/* Postive Logic */
TCCR0A |= (1<<COM0A1);
+ motor_set_target_speed(MOTOR_TARGET_SPEED_INIT);
/* Enable Interrupts */
sei();
}
@@ -50,21 +54,40 @@ void motor_init(void)
/* This function starts the motor rotation */
void motor_start(void)
{
- OCR0A = 115;
+ motor_set_speed(MOTOR_SPEED_MIN);
start_codewheel_timer_task();
}
/* This function stops the motor rotation */
void motor_stop(void)
{
- OCR0A = 0;
+ motor_set_speed(MOTOR_SPEED_STOP);
+ motor_set_target_speed(MOTOR_SPEED_STOP);
stop_codewheel_timer_task();
}
+
+/* This function sets the motor speed */
+void motor_set_speed(uint8_t value)
+{
+ if(value >= 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