hi kuade,
if you didn't plan to use <DigiUSB> library in your sketch, this is typically an application you can do very easily with the <RcSeq> library which is designed for servo sequences.
<RcSeq> library is delivered with the DigiSpark IDE.
Please find below a sketch for your application. To launch the sequence, simply put P5 to ground (using a push-button). Servos are connected to P1 and P2.
You can also launch the sequence from a standard Radio-Control Set using P0.
All the angles and motion durations are tunable using #define (see code):
/*
This sketch controls 2 servos to sequence an arm for track.
Based on <RcSeq> library
RC Navy 2013
http://p.loussouarn.free.fr
Steps:
=====
1. Servo 1 pushes up que lever (delay of 1500msec needed)
2. Servo 2 pushes tone arm to first track on record
3. Servo 2 retract to original position
4. Servo 1 pulls que lever to lower tone arm
*/
/*************************************************/
/* STEP #1: Include the 4 needed libraries */
/*************************************************/
#include <RcSeq.h>
#include <TinyPinChange.h>
#include <SoftRcPulseIn.h>
#include <SoftRcPulseOut.h>
/*****************************************************/
/* STEP #2: RC command signal Enumeration */
/*****************************************************/
enum {RC_SIGNAL=0, SIGNAL_NB}; /* Even if a Radio-Control is not used, a signal shall be declared with the current version of <RcSeq> library */
/****************************************************************/
/* STEP #3: Enumeration of the RC stick positions */
/****************************************************************/
enum {RC_PULSE_LEVEL_MINUS_2, RC_PULSE_LEVEL_MINUS_1, RC_PULSE_LEVEL_PLUS_1, RC_PULSE_LEVEL_PLUS_2, RC_PULSE_NB};
/*****************************************************************/
/* STEP #4: Enumeration of the servos used in the sequence */
/*****************************************************************/
enum {QUE_LEVER_SERVO=0, TONE_ARM_SERVO , SERVO_NB};
/*****************************************************************/
/* STEP #5: RC command signals Digital Pin assignment */
/*****************************************************************/
#define RECEIVER_SIGNAL_PIN 0
/*****************************************************************/
/* STEP #6: Servos Digital Pins assignment */
/*****************************************************************/
#define QUE_LEVER_SERVO_PIN 1
#define TONE_ARM_SERVO_PIN 2
#define PUSH_BUTTON 5 /* Hack to launch the sequence without Radio-Control set, just using a push button */
/**************************************************************************************/
/* STEP #7: Declaration of the angles of the servos for the different motions (in °) */
/**************************************************************************************/
#define PULLED_QUE_LEVER_POS 70 /* 90 is servo neutral */
#define PUSHED_QUE_LEVER_POS 110 /* 90 is servo neutral */
#define PULLED_TONE_ARM_POS 90 /* 90 is servo neutral */
#define PUSHED_TONE_ARM_POS 120 /* 90 is servo neutral */
/***************************************************************************************************************************************/
/* STEP #8: Do a temporal diagram showing the start up and the duration of each motions of each servo */
/***************************************************************************************************************************************/
/*
All the start up values (time stamp) have as reference the moment of the sequence startup order (t=0).
QUE_LEVER SERVO MOTION SERVOS KEEP POSITION (WAIT) TONE_ARM SERVO MOTION SERVOS KEEP POSITION (WAIT) TONE_ARM SERVO MOTION SERVOS KEEP POSITION (WAIT) QUE_LEVER SERVO MOTION
Order <--PUSH_QUE_LEVER_MOTION_DURATION_MS--> <--WAIT_DELAY_AFTER_PUSH_QUE_LEVER_MS--> <--PUSH_TONE_ARM_DURATION_MS--><--WAIT_DELAY_AFTER_PUSH_TONE_ARM_MS--><--RETRACT_TONE_ARM_MOTION_DURATION_MS--><--WAIT_DELAY_AFTER_RETRACT_TONE_ARM_MS--><--QUE_LEVER_PULL_MOTION_DURATION_MS-->
|---------------|---------------------------------------|----------------------------------------|-------------------------------|--------------------------------------|----------------------------------------|-----------------------------------------|--------------------------------------|-->Time Axis
0 START_PUSH_QUE_LEVER_DELAY_MS START_PUSH_TONE_ARM_MS START_RETRACT_TONE_ARM_DELAY_MS START_PULL_QUE_LEVER_DELAY_MS
*/
/**************************************************************************************************************************************************/
/* STEP #9: With the help of the temporal diagram, declare start up time, the motion duration of servo and optional delay */
/**************************************************************************************************************************************************/
/* Tune below motion duration (ms). Do not forget to add a trailer 'UL' for each value to force them in Usigned Long type */
#define START_PUSH_QUE_LEVER_DELAY_MS 0UL /* 0 for immediate start up, but you can put a delay here. Ex: 2000UL, will delay the startup of the whole sequence after 2 seconds */
#define PUSH_QUE_LEVER_MOTION_DURATION_MS 1500UL /* Tune here motion duration */
#define WAIT_DELAY_AFTER_PUSH_QUE_LEVER_MS 500UL /* Tune here delay if needed */
#define START_PUSH_TONE_ARM_MS (START_PUSH_QUE_LEVER_DELAY_MS + PUSH_QUE_LEVER_MOTION_DURATION_MS + WAIT_DELAY_AFTER_PUSH_QUE_LEVER_MS)
#define PUSH_TONE_ARM_DURATION_MS 2000UL /* Tune here motion duration */
#define WAIT_DELAY_AFTER_PUSH_TONE_ARM_MS 500UL /* Tune here delay if needed */
#define START_RETRACT_TONE_ARM_DELAY_MS (START_PUSH_TONE_ARM_MS + PUSH_TONE_ARM_DURATION_MS + WAIT_DELAY_AFTER_PUSH_TONE_ARM_MS)
#define RETRACT_TONE_ARM_MOTION_DURATION_MS 1500UL /* Tune here motion duration */
#define WAIT_DELAY_AFTER_RETRACT_QUE_LEVER_MS 500UL /* Tune here delay if needed */
#define START_PULL_QUE_LEVER_DELAY_MS (START_RETRACT_TONE_ARM_DELAY_MS + RETRACT_TONE_ARM_MOTION_DURATION_MS + WAIT_DELAY_AFTER_RETRACT_QUE_LEVER_MS)
#define QUE_LEVER_PULL_MOTION_DURATION_MS 2000UL /* Tune here motion duration */
/********************************************************************************************************************/
/* STEP #10: Declare here the percentage of motion to be performed at half speed for servo start up and stop */
/********************************************************************************************************************/
#define START_STOP_PER_CENT 5 /* Percentage of motion performed at half speed for servo start and servo stop (Soft start and Soft stop) */
/************************************************************************************************************/
/* STEP #11: Use a "SequenceSt_t" structure table to declare the servo sequence */
/* For each table entry, arguments are: */
/* - Servo Index */
/* - Initial Servo Position in ° */
/* - Final Servo Position in ° */
/* - Motion Start Time Stamp in ms */
/* - Motion duration in ms between initial and final position */
/* - Percentage of motion performed at half speed for servo start and servo stop (Soft start and Soft stop) */
/************************************************************************************************************/
SequenceSt_t ArmSequence[] PROGMEM = {
/* 1. Servo 1 pushes up que lever (delay of 1500msec needed) */
MOTION_WITH_SOFT_START_AND_STOP(QUE_LEVER_SERVO, PULLED_QUE_LEVER_POS, PUSHED_QUE_LEVER_POS, START_PUSH_QUE_LEVER_DELAY_MS, PUSH_QUE_LEVER_MOTION_DURATION_MS, START_STOP_PER_CENT)
/* 2. Servo 2 pushes tone arm to first track on record */
MOTION_WITH_SOFT_START_AND_STOP(TONE_ARM_SERVO, PULLED_TONE_ARM_POS, PUSHED_TONE_ARM_POS, START_PUSH_TONE_ARM_MS, PUSH_TONE_ARM_DURATION_MS, START_STOP_PER_CENT)
/* 3. Servo 2 retract to original position */
MOTION_WITH_SOFT_START_AND_STOP(TONE_ARM_SERVO, PUSHED_TONE_ARM_POS, PULLED_TONE_ARM_POS, START_RETRACT_TONE_ARM_DELAY_MS, RETRACT_TONE_ARM_MOTION_DURATION_MS, START_STOP_PER_CENT)
/* 4. Servo 1 pulls que lever to lower tone arm */
MOTION_WITH_SOFT_START_AND_STOP(QUE_LEVER_SERVO, PUSHED_QUE_LEVER_POS, PULLED_QUE_LEVER_POS, START_PULL_QUE_LEVER_DELAY_MS, QUE_LEVER_PULL_MOTION_DURATION_MS, START_STOP_PER_CENT)
};
void setup()
{
/***************************************************************************/
/* STEP #12: Init <RcSeq> library */
/***************************************************************************/
RcSeq_Init();
/**************************************************************************************/
/* STEP #13: declare the RC signal(s) with their digital pin number */
/**************************************************************************************/
RcSeq_DeclareSignal(RC_SIGNAL, RECEIVER_SIGNAL_PIN);
/*********************************************************************************************************/
/* STEP #14: declare the RC signal RC is associated with a stick which has RC_PULSE_POSTION_NB positions */
/*********************************************************************************************************/
RcSeq_DeclareStick(RC_SIGNAL, 1000, 2000, RC_PULSE_NB);
/****************************************************************************************/
/* STEP #15: declare the servo command signals with their digital pin number */
/****************************************************************************************/
RcSeq_DeclareServo(QUE_LEVER_SERVO, QUE_LEVER_SERVO_PIN);
RcSeq_DeclareServo(TONE_ARM_SERVO, TONE_ARM_SERVO_PIN);
/**************************************************************************************************************************/
/* STEP #16: declare the sequence command signal, the stick level, and the sequence to call */
/**************************************************************************************************************************/
RcSeq_DeclareCommandAndSequence(RC_SIGNAL, RC_PULSE_LEVEL_PLUS_2, RC_SEQUENCE(ArmSequence)); /* Stick at Level Plus 2 during at least 250 ms */
pinMode(PUSH_BUTTON, INPUT);
digitalWrite(PUSH_BUTTON, HIGH); /* Enable Pull-up */
}
void loop()
{
/***********************************************************************************************************************************/
/* STEP #17: call the refresh function inside the loop() to catch RC commands and to manage the servo positions */
/***********************************************************************************************************************************/
RcSeq_Refresh();
/***************************************************************************************************************/
/* STEP #18: optionnally, the sequence can be launched directly by calling the RcSeq_LaunchSequence() function */
/***************************************************************************************************************/
/* Launch the sequence by pressing the external Push-Button: this allows to test the servo sequence without Radio-Control set */
if(digitalRead(PUSH_BUTTON) == LOW)
{
RcSeq_LaunchSequence(ArmSequence); /* No debounce needed: Push-Button Debounce is done inside Rc_Seq */
}
}
In the current DigiSpark IDE, compiler may complain about some header file path:
1) In /libraires/DigisparkRcSeq/RcSeq.h, please replace:
#include "../SoftRcPulseIn/SoftRcPulseIn.h"
#include "../SoftRcPulseOut/SoftRcPulseOut.h"
#include "../TinyPinChange/TinyPinChange.h"
with:
#include <SoftRcPulseIn.h>
#include <SoftRcPulseOut.h>
#include <TinyPinChange.h>
----------------------------------------------------------------
2) In /libraires/DigiSparkSoftRcPulseIn/SoftRcPulseIn, please replace:
#include "../SoftRcPulseIn/SoftRcPulseIn.h"
with:
#include <SoftRcPulseIn.h>
After, these fixes, it compiles.
Sketch tested with a DigiSpark: works for me.
Enjoy,
RC Navy
