Commit 814c10e4 authored by Xoaquin Castrelo's avatar Xoaquin Castrelo
Browse files

Added function to get servo angle.

parent 783e78eb
......@@ -4,7 +4,6 @@
#include <avr/interrupt.h>
#include <stdint.h>
#define NUM_SERVOS 3
#define MIN_PULSE 999
#define MAX_PULSE 4999
......@@ -18,6 +17,7 @@ struct Servo
const uint8_t pinMask; // the pin mask for the servo
uint16_t tempValue; // stores a temporary value before it's written to the servo
uint16_t value; // current pulse width value
int16_t angle; // the angle in arc-minutes
};
static Servo servos[NUM_SERVOS] = {
......@@ -43,7 +43,23 @@ void initServos()
void setServoAngle(uint8_t servo, int16_t arcMin)
{
if (0 <= servo && servo < NUM_SERVOS)
{
if (arcMin < -180 * 60)
arcMin = -180 * 60;
else if (arcMin > 180 * 60)
arcMin = 180 * 60;
servos[servo].tempValue = MIN_PULSE + (int32_t) (arcMin + 180 * 60) * (MAX_PULSE - MIN_PULSE) / (360 * 60);
servos[servo].angle = arcMin;
}
}
int16_t getServoAngle(uint8_t servo)
{
if (0 <= servo && servo < NUM_SERVOS)
return servos[servo].angle;
else
return 0;
}
void updateServos()
......
......@@ -3,6 +3,8 @@
#include <stdint.h>
#define NUM_SERVOS 3
/**
* Initialises the servos.
*/
......@@ -16,6 +18,11 @@ void initServos();
*/
void setServoAngle(uint8_t servo, int16_t arcMin);
/**
* Gets the angle of a servo.
*/
int16_t getServoAngle(uint8_t servo);
/**
* Writes the servo values out to the servos.
*/
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment