Commit 90559b09 authored by jp7g21's avatar jp7g21
Browse files

Merge remote-tracking branch 'refs/remotes/origin/master'

parents cd5086cc 6d64383d
...@@ -3,13 +3,15 @@ ...@@ -3,13 +3,15 @@
#include <stdint.h> #include <stdint.h>
#define NUM_ARGS 3
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif
struct command { struct command {
char comm_ch; /* Character of command (ie 's', 'v', 'd') */ char comm_ch; /* Character of command (ie 's', 'v', 'd') */
int16_t arg[2]; int16_t arg[NUM_ARGS];
}; };
uint8_t uart_add_ch(char c); uint8_t uart_add_ch(char c);
......
...@@ -4,7 +4,6 @@ ...@@ -4,7 +4,6 @@
#include <avr/interrupt.h> #include <avr/interrupt.h>
#include <stdint.h> #include <stdint.h>
#define NUM_SERVOS 3
#define MIN_PULSE 999 #define MIN_PULSE 999
#define MAX_PULSE 4999 #define MAX_PULSE 4999
...@@ -18,6 +17,7 @@ struct Servo ...@@ -18,6 +17,7 @@ struct Servo
const uint8_t pinMask; // the pin mask for the 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 tempValue; // stores a temporary value before it's written to the servo
uint16_t value; // current pulse width value uint16_t value; // current pulse width value
int16_t angle; // the angle in arc-minutes
}; };
static Servo servos[NUM_SERVOS] = { static Servo servos[NUM_SERVOS] = {
...@@ -43,7 +43,23 @@ void initServos() ...@@ -43,7 +43,23 @@ void initServos()
void setServoAngle(uint8_t servo, int16_t arcMin) void setServoAngle(uint8_t servo, int16_t arcMin)
{ {
if (0 <= servo && servo < NUM_SERVOS) 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].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() void updateServos()
......
...@@ -3,6 +3,8 @@ ...@@ -3,6 +3,8 @@
#include <stdint.h> #include <stdint.h>
#define NUM_SERVOS 3
/** /**
* Initialises the servos. * Initialises the servos.
*/ */
...@@ -16,6 +18,11 @@ void initServos(); ...@@ -16,6 +18,11 @@ void initServos();
*/ */
void setServoAngle(uint8_t servo, int16_t arcMin); 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. * Writes the servo values out to the servos.
*/ */
......
...@@ -6,19 +6,39 @@ ...@@ -6,19 +6,39 @@
enum State enum State
{ {
STATE_DELAY, STATE_READY STATE_READY = 0, STATE_DELAY
}; };
static State state = STATE_READY; static State state = STATE_READY;
static uint32_t delayEnd = 0; static uint32_t delayEnd = 0;
static int16_t servoVelocities[NUM_SERVOS];
static uint32_t velocitiesEndMillis[NUM_SERVOS];
static int16_t startAngles[NUM_SERVOS];
static uint32_t startMillis[NUM_SERVOS];
void parseCommand(command *comm) void parseCommand(command *comm)
{ {
uint32_t currentMillis = getCurrentMillis();
switch (comm->comm_ch) switch (comm->comm_ch)
{ {
case 's': case 's':
setServoAngle(comm->arg[0] - 1, comm->arg[1] * 60); if (0 <= comm->arg[0] - 1 && comm->arg[0] - 1 < NUM_SERVOS)
updateServos(); {
setServoAngle(comm->arg[0] - 1, comm->arg[1] * 60);
updateServos();
servoVelocities[comm->arg[0] - 1] = 0;
}
break;
case 'v':
if (0 <= comm->arg[0] - 1 && comm->arg[0] - 1 < NUM_SERVOS)
{
servoVelocities[comm->arg[0] - 1] = comm->arg[1];
velocitiesEndMillis[comm->arg[0] - 1] = currentMillis + comm->arg[2];
startAngles[comm->arg[0] - 1] = getServoAngle(comm->arg[0] - 1);
startMillis[comm->arg[0] - 1] = currentMillis;
}
break; break;
case 'd': case 'd':
delayEnd = getCurrentMillis() + comm->arg[0]; delayEnd = getCurrentMillis() + comm->arg[0];
...@@ -29,6 +49,8 @@ void parseCommand(command *comm) ...@@ -29,6 +49,8 @@ void parseCommand(command *comm)
void updateState() void updateState()
{ {
uint32_t currentMillis = getCurrentMillis();
switch (state) switch (state)
{ {
case STATE_READY: case STATE_READY:
...@@ -44,11 +66,29 @@ void updateState() ...@@ -44,11 +66,29 @@ void updateState()
break; break;
} }
case STATE_DELAY: case STATE_DELAY:
if (getCurrentMillis() > delayEnd) if (currentMillis > delayEnd)
state = STATE_READY; state = STATE_READY;
break; break;
} }
for (uint8_t i = 0; i < NUM_SERVOS; i++)
{
if (servoVelocities[i] != 0)
{
if (currentMillis < velocitiesEndMillis[i])
{
int16_t newAngle = startAngles[i] + (int32_t) servoVelocities[i] * 60 * (int32_t) (currentMillis - startMillis[i]) / 1000;
setServoAngle(i, newAngle);
}
else
{
setServoAngle(i, startAngles[i] + (int32_t) servoVelocities[i] * 60 * (int32_t) (velocitiesEndMillis[i] - startMillis[i]) / 1000);
servoVelocities[i] = 0;
}
}
}
updateServos();
} }
Supports Markdown
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