Skip to content
Permalink
master
Switch branches/tags
Go to file
 
 
Cannot retrieve contributors at this time
/**
* @file rc_test_servos.c
* @example rc_test_servos
*
*
* Demonstrates use of pru to control servos. This program
* operates in 4 different modes. See the option list below for how to select an
* operational mode from the command line. In all modes the power rail is
* enabled to power the servos and the program will refuse to start unless a
* charged 2-cell LiPo is plugged into the while balance jack and detected by
* the ADC.
*
* NORM: uses rc_servo_send_pulse_normalized() to set one or all servo
* positions to a value from -1.5 to 1.5 corresponding to their extended range.
* -1 to 1 is considered the "safe" normal range as some servos will not go
* beyond this. Test your servos incrementally to find their safe range.
*
* MICROSECONDS: You can also specify your own pulse width in microseconds (us).
* This uses the rc_servo_send_pulse_us() function.
*
* SWEEP: This is intended to gently sweep a servo back and forth about the
* center position. Specify a range limit as a command line argument as
* described below. This also uses the rc_servo_send_pulse_normalized()
* function.
*
* RADIO: Uses a single user-selected channel of a DSM radio to control one
* or all of the servo channels. This is really just for testing, use the
* rc_dsm_passthrough example for controlling multiple servos, or better yet
* write an application-specific program for your robot. After all, this program
* is just for testing!!!!
*
* @author James Strawson
* @date 3/20/2018
*/
#include <stdio.h>
#include <getopt.h>
#include <stdlib.h> // for atoi
#include <signal.h>
#include <rc/time.h>
#include <rc/adc.h>
#include <rc/dsm.h>
#include <rc/servo.h>
static int running;
typedef enum test_mode_t{
DISABLED,
NORM,
MICROSECONDS,
SWEEP,
RADIO
}test_mode_t;
static void __print_usage(void)
{
printf("\n");
printf(" Options\n");
printf(" -c {channel} Specify one channel from 1-8.\n");
printf(" Otherwise all channels will be driven equally\n");
printf(" -f {hz} Specify pulse frequency, otherwise 50hz is used\n");
printf(" -p {position} Drive servo to a position between -1.5 & 1.5\n");
printf(" -w {width_us} Send pulse width in microseconds (us)\n");
printf(" -s {limit} Sweep servo back/forth between +- limit\n");
printf(" Limit can be between 0 & 1.5\n");
printf(" -r {ch} Use DSM radio channel {ch} to control servo\n");
printf(" -h Print this help messege \n\n");
printf("sample use to center servo channel 1:\n");
printf(" rc_test_servo -c 1 -p 0.0\n\n");
}
// interrupt handler to catch ctrl-c
static void __signal_handler(__attribute__ ((unused)) int dummy)
{
running=0;
return;
}
int main(int argc, char *argv[])
{
double servo_pos=0;
double sweep_limit=0;
uint64_t dsm_nanos=0;
int width_us=0;
int ch=0; // channel to test, 0 means all channels
double direction = 1; // switches between 1 & -1 in sweep mode
test_mode_t mode = DISABLED; //start mode disabled
int frequency_hz = 50; // default 50hz frequency to send pulses
int i, c, radio_ch;
// parse arguments
opterr = 0;
while ((c = getopt(argc, argv, "c:f:p:w:s:r:h")) != -1){
switch (c){
// channel option
case 'c':
ch = atoi(optarg);
if(ch<RC_SERVO_CH_MIN || ch>RC_SERVO_CH_MAX){
fprintf(stderr,"ERROR channel option must be between %d and %d\n", RC_SERVO_CH_MIN, RC_SERVO_CH_MAX);
return -1;
}
break;
// pulse frequency option
case 'f':
frequency_hz = atoi(optarg);
if(frequency_hz<1){
fprintf(stderr,"Frequency option must be >=1\n");
return -1;
}
break;
// position option
case 'p':
// make sure only one mode in requested
if(mode!=DISABLED){
__print_usage();
return -1;
}
servo_pos = atof(optarg);
if(servo_pos>1.5 || servo_pos<-1.5){
fprintf(stderr,"Servo position must be from -1.5 to 1.5\n");
return -1;
}
mode = NORM;
break;
// width in microseconds option
case 'w':
// make sure only one mode in requested
if(mode!=DISABLED){
__print_usage();
return -1;
}
width_us = atof(optarg);
if(width_us<10){
fprintf(stderr,"ERROR: Width in microseconds must be >10\n");
return -1;
}
mode = MICROSECONDS;
break;
// sweep mode option
case 's':
// make sure only one mode in requested
if(mode!=DISABLED){
__print_usage();
return -1;
}
sweep_limit = atof(optarg);
if(sweep_limit>1.5 || sweep_limit<-1.5){
fprintf(stderr,"ERROR: Sweep limit must be from -1.5 to 1.5\n");
return -1;
}
mode = SWEEP;
servo_pos = 0.0;
break;
// radio mode option
case 'r':
// make sure only one mode in requested
if(mode!=DISABLED){
__print_usage();
return -1;
}
radio_ch = atoi(optarg);
if(radio_ch<1 || radio_ch>RC_MAX_DSM_CHANNELS){
fprintf(stderr,"ERROR radio channel option must be between 1 and %d\n", RC_MAX_DSM_CHANNELS);
return -1;
}
mode = RADIO;
break;
// help mode
case 'h':
__print_usage();
return 0;
default:
printf("\nInvalid Argument \n");
__print_usage();
return -1;
}
}
// if the user didn't give enough arguments, exit
if(mode==DISABLED){
fprintf(stderr,"\nNot enough input arguments\n");
__print_usage();
return -1;
}
// set signal handler so the loop can exit cleanly
signal(SIGINT, __signal_handler);
running=1;
// read adc to make sure battery is connected
if(rc_adc_init()){
fprintf(stderr,"ERROR: failed to run rc_adc_init()\n");
return -1;
}
if(rc_adc_batt()<6.0){
fprintf(stderr,"ERROR: battery disconnected or insufficiently charged to drive servos\n");
return -1;
}
rc_adc_cleanup();
// initialize PRU
if(rc_servo_init()) return -1;
// start radio if necessary
if(mode==RADIO){
if(rc_dsm_init()==-1) return -1;
printf("Waiting for first DSM packet\n");
fflush(stdout);
while(rc_dsm_is_new_data()==0){
if(running==0) return 0;
rc_usleep(50000);
}
}
// turn on power
printf("Turning On 6V Servo Power Rail\n");
rc_servo_power_rail_en(1);
// print out what the program is doing
printf("\n");
if(ch==0) printf("Sending on all channels.\n");
else printf("Sending only to channel %d.\n", ch);
switch(mode){
case NORM:
printf("Using rc_servo_send_pulse_normalized\n");
printf("Normalized Signal: %f Pulse Frequency: %d\n", servo_pos, frequency_hz);
break;
case MICROSECONDS:
printf("Using rc_servo_send_pulse_us\n");
printf("Pulse_width: %d Pulse Frequency: %d\n", width_us, frequency_hz);
break;
case SWEEP:
printf("Sweeping servos back/forth between +-%f\n", sweep_limit);
printf("Pulse Frequency: %d\n", frequency_hz);
break;
case RADIO:
printf("Listening for DSM radio signal\n");
printf("Pulse Frequency: %d\n", frequency_hz);
break;
default:
// should never get here
fprintf(stderr,"ERROR invalid mode enum\n");
return -1;
}
// Main loop runs at frequency_hz
while(running){
switch(mode){
case NORM:
if(rc_servo_send_pulse_normalized(ch,servo_pos)==-1) return -1;
break;
case MICROSECONDS:
if(rc_servo_send_pulse_us(ch, width_us)==-1) return -1;
break;
case SWEEP:
// increase or decrease position each loop
// scale with frequency
servo_pos += direction * sweep_limit / frequency_hz;
// reset pulse width at end of sweep
if(servo_pos>sweep_limit){
servo_pos = sweep_limit;
direction = -1;
}
else if(servo_pos < (-sweep_limit)){
servo_pos = -sweep_limit;
direction = 1;
}
// send result
if(rc_servo_send_pulse_normalized(ch,servo_pos)==-1) return -1;
break;
case RADIO:
dsm_nanos = rc_dsm_nanos_since_last_packet();
if(dsm_nanos > 200000000){
printf("\rSeconds since last DSM packet: %.2f ", dsm_nanos/1000000000.0);
}
else{
servo_pos = rc_dsm_ch_normalized(radio_ch);
// bound the signal to the escs
if(servo_pos<-1.5) servo_pos=-1.5;
if(servo_pos>1.5) servo_pos=1.5;
// send pulse
if(rc_servo_send_pulse_normalized(ch,servo_pos)==-1) return -1;
// print info
printf("\r");// keep printing on same line
// print framerate
printf("%d/", rc_dsm_resolution());
// print num channels in use
printf("%d-ch ", rc_dsm_channels());
//print all channels
for(i=0; i<rc_dsm_channels(); i++){
printf("%d:% 0.2f ", i+1, rc_dsm_ch_normalized(i+1));
}
}
break;
default:
fprintf(stderr,"ERROR unhandled mode\n");
return -1;
}
// sleep roughly enough to maintain frequency_hz
rc_usleep(1000000/frequency_hz);
}
rc_usleep(50000);
// turn off power rail and cleanup
rc_servo_power_rail_en(0);
rc_servo_cleanup();
rc_dsm_cleanup();
return 0;
}