void aimAtTargetFORWARD(int error)
{
printf("Attempting to aim at target! Error: %d\n", error);
setRun(RIGHT_WHEEL_INDEX, 1);
setRun(LEFT_WHEEL_INDEX, 1);
setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY - (error * 50) + (ADJUST_RATE / 2));
setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY + (error * 50) - (ADJUST_RATE / 2));
}
static void motorTask(void *params) {
Motor *motor = (Motor*)params;
portTickType xLastWakeTime;
s32 pos, targetPos;
u8 stopped;
s32 currentDuty = 0;
u16 maxDutyChange = MAX_DUTY * motor->pollingPeriod / ACCEL_TIME;
xLastWakeTime = xTaskGetTickCount();
for (;;) {
xSemaphoreTake(motor->lock, portMAX_DELAY);
pos = getPosition(motor->currentPosition);
targetPos = motor->targetPosition;
stopped = motor->stopped;
xSemaphoreGive(motor->lock);
if (stopped) {
// immediately stop the motor
if (currentDuty >= maxDutyChange)
currentDuty -= maxDutyChange;
else if (currentDuty <= -maxDutyChange)
currentDuty += maxDutyChange;
else
currentDuty = 0;
setDuty(motor, currentDuty);
} else if (targetPos > pos) {
// We have to increase the position to reach the target
setDirection(motor->currentPosition, Up);
currentDuty = min(maxDutyAtDistance(targetPos - pos),
currentDuty + maxDutyChange);
setDuty(motor, currentDuty);
} else if (targetPos < pos) {
// We have to decrease the position to reach the target
setDirection(motor->currentPosition, Down);
currentDuty = -min(maxDutyAtDistance(pos - targetPos),
-currentDuty + maxDutyChange);
setDuty(motor, currentDuty);
} else {
// We have reached the target
setDirection(motor->currentPosition, Unknown);
currentDuty = 0;
setDuty(motor, currentDuty);
}
vTaskDelayUntil(&xLastWakeTime, motor->pollingPeriod);
}
}
//---------------------------------------------------
TremoloDSP::TremoloDSP( XMLParser& parser, XMLNode* tremoloNode )
: DSP( MONKY_DSP_TYPE_TREMOLO )
{
const float NO_VALUE_SPECIFIED = -1000000000.0f;
parser.validateXMLAttributes( tremoloNode, "", "type,frequency,depth,shape,timeSkewing,duty,flatness,phase,spread" );
float frequency = parser.getXMLAttributeAsFloat( tremoloNode, "frequency", NO_VALUE_SPECIFIED );
float depth = parser.getXMLAttributeAsFloat( tremoloNode, "depth", NO_VALUE_SPECIFIED );
float shape = parser.getXMLAttributeAsFloat( tremoloNode, "shape", NO_VALUE_SPECIFIED );
float timeSkewing = parser.getXMLAttributeAsFloat( tremoloNode, "timeSkewing", NO_VALUE_SPECIFIED );
float duty = parser.getXMLAttributeAsFloat( tremoloNode, "duty", NO_VALUE_SPECIFIED );
float flatness = parser.getXMLAttributeAsFloat( tremoloNode, "flatness", NO_VALUE_SPECIFIED );
float phase = parser.getXMLAttributeAsFloat( tremoloNode, "phase", NO_VALUE_SPECIFIED );
float spread = parser.getXMLAttributeAsFloat( tremoloNode, "spread", NO_VALUE_SPECIFIED );
if( frequency != NO_VALUE_SPECIFIED )
setFrequency( frequency );
if( depth != NO_VALUE_SPECIFIED )
setDepth( depth );
if( shape != NO_VALUE_SPECIFIED )
setShape( shape );
if( timeSkewing != NO_VALUE_SPECIFIED )
setTimeSkewing( timeSkewing );
if( duty != NO_VALUE_SPECIFIED )
setDuty( duty );
if( flatness != NO_VALUE_SPECIFIED )
setFlatness( flatness );
if( phase != NO_VALUE_SPECIFIED )
setPhase( phase );
if( spread != NO_VALUE_SPECIFIED )
setSpread( spread );
}
//COMMANDE PWM /////////////////////////////////////////////////////////////
void Servo::commande(const std_msgs::Float64::ConstPtr& msg)
{
float input = msg->data;
float commandeNs = zero + input * k;
if(type== "direction" || "moteur" || "selecteur")
{
if(commandeNs < dutyMax && commandeNs > dutyMin)
{
duty = (int)commandeNs;
setDuty(duty);
}
else
{
std::cout << "(Servo::commande)["<< type <<"] valeur limites depassees" << std::endl;
}
}
else
{
std::cout << "(Servo::commande) : erreur de type !" << std::endl;
}
}
// CREATE Timer T1 PWM to drive inverter for regulated Geiger tube voltage
void Inverter::initPWM() {
TCCR1A = 0; // disable all PWM on Timer1 whilst we set it up
DDRB |= _BV(PB5); // Set PB5 as output (PB5 is OC1A)
ICR1 = F_CPU / freq; // set the frequency FCPU/(ICR1 * PRESCALLING) Hz . Prescalling set to 1X
setDuty(duty);
TCCR1B = (1 << WGM13) | (1<<WGM12) | (1 << CS10); //Fast PWM mode via ICR1, with no prescaling (CS11 = 8x, CS10 = 1x)
TCCR1A |= (1 << COM1A1) | (1<< CS11); // set none-inverting mode and start PWM
}
void DutyUnit::loadState(const SaveState::SPU::Duty &dstate, const unsigned nr1, const unsigned nr4, const unsigned long cc) {
nextPosUpdate = std::max(dstate.nextPosUpdate, cc);
pos = dstate.pos & 7;
setDuty(nr1);
period = toPeriod((nr4 << 8 & 0x700) | dstate.nr3);
enableEvents = true;
setCounter();
}
PwmSysfsDriverNode::PwmSysfsDriverNode(ros::NodeHandle const& nh,
ros::NodeHandle const& nh_rel):
nh_(nh), nh_rel_(nh_rel)
{
// get reqired path to sysfs pwm directory parameter
std::string pwm_sysfs_dir;
if (!nh_rel_.getParam("pwm_sysfs_dir", pwm_sysfs_dir)) {
ROS_FATAL("Path to sysfs pwm directory required.");
ros::shutdown();
return;
}
// attemp to create driver
try {
driver_.reset(new PwmSysfsDriver(pwm_sysfs_dir));
}
catch (PwmSysfsDriverException e) {
driver_.reset();
ROS_FATAL("Failed to initialize driver: %s", e.what());
ros::shutdown();
return;
}
// set initial pwm values
bool enable;
if (nh_rel_.getParam("initial_enable", enable))
setEnable(enable);
bool invert_polarity;
if (nh_rel_.getParam("initial_invert_polarity", invert_polarity))
setInvertPolarity(invert_polarity);
double period;
if (nh_rel_.getParam("initial_period", period))
setPeriod(period);
double duty;
if (nh_rel_.getParam("initial_duty", duty))
setDuty(duty);
// boolean pwm enable
enable_sub_ = nh_.subscribe("enable", 10,
&PwmSysfsDriverNode::enableCallback, this);
// boolean pwm invert polarity
invert_polarity_sub_ =
nh_.subscribe("invert_polarity", 10,
&PwmSysfsDriverNode::invertPolarityCallback, this);
// pwm cycle period, in nanoseconds
period_sub_ = nh_.subscribe("period", 10,
&PwmSysfsDriverNode::periodCallback, this);
// pwm duty cycle fraction, 0 to 1
duty_sub_ = nh_.subscribe("duty", 10,
&PwmSysfsDriverNode::dutyCallback, this);
}
void SPIPWMworker(void)
{
unsigned char token, instruction, fInst=0,fb=0,bf=0;
initSPI();
SSP2BUF = 0xFD;
enablePWM();
while (1)
{
while(PORTCbits.RC0 ==0)//CS LOW
{
if(SSP2STATbits.BF) // we received something :)
{
bf=1;
if(fb==0)
{
token = SSP2BUF;
instruction = token & SPIEEMASK;
}
fb=1;
switch (instruction)
{
case 0:
if(fInst)
{
if(fInst==1)setPeriod(SSP2BUF);
fInst++;
}
else
{
fInst=1;
}
SSP2BUF = PWMperiod;
break;
case 1:
if(fInst)
{
if(fInst==1)setDuty(SSP2BUF);
fInst++;
}
else
{
fInst=1;
}
SSP2BUF = PWMduty;
break;
}
}
}
if(bf) //CS HIGH
{
fb=0;
bf=0;
fInst = 0;
}
}
}
// check tube voltage and adjust duty cycle to match output given threshold level
// return false on critical error (when we hit the duty max), true if all ok
bool Inverter::adjustDutyCycle(int measuredVoltage) {
if (measuredVoltage > highestVoltage) {
highestVoltage = measuredVoltage;
highestVoltageDuty = duty;
}
if ( (measuredVoltage >= target + tolerance) && (duty > dutymin)) {
setDuty(duty - 1); // we need to decrease duty cycle to decrease voltage
}
if ( (measuredVoltage <= target - tolerance) && (duty < dutymax)) {
setDuty(duty + 1); // we need to increase duty cycle to increase voltage
}
if (duty == INVERTER_DUTY_MAX) {
setDuty(highestVoltageDuty);
return false; // error: no place to increase, yet we have not reached the target voltage
}
else return true;
}
int main()
{
uint8_t i;
cli();
wdt_enable(WDTO_60MS);
for (i = 0; i < 8; i++) {
PORTA = PORTA >> 1;
PORTA |= ((readEEPROM(i) - '0') & 0x01) << 7;
}
for (i = 8; i < 16; i++) {
PORTC = PORTC >> 1;
PORTC |= ((readEEPROM(i) - '0') & 0x01) << 7;
}
DDRA = 0xff;
DDRC = 0xff;
for (stackTail = EEPROM_SIZE - 1; readEEPROM(stackTail) != 0xff; stackTail--);
status = ((uint16_t) PORTC) << 8 | (uint16_t) PORTA;
initUSART();
setDuty();
initTimer0();
initTimer2();
sei();
printf("\nEntering the main loop\n");
while (1) {
wdt_reset();
}
return 0;
}
void rotate(int time){
if (time > 0){
setDuty(LEFT_WHEEL_INDEX, MAXIMUM_DUTY);
setDuty(RIGHT_WHEEL_INDEX, MAXIMUM_DUTY);
}
else if (time < 0){
setDuty(LEFT_WHEEL_INDEX, MINIMUM_DUTY);
setDuty(RIGHT_WHEEL_INDEX, MINIMUM_DUTY);
}
usleep(microsecondsToMilliseconds(abs(time)));
setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY);
setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY);
}
void aimAtTarget(double error){
if (error == 0){
return;
}
if (error < 0){
//Turn left
setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY-ADJUST_RATE);
setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY-ADJUST_RATE);
}
else if (error > 0){
//Turn right
setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY+ADJUST_RATE);
setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY+ADJUST_RATE);
}
setRun(RIGHT_WHEEL_INDEX, 1);
setRun(LEFT_WHEEL_INDEX, 1);
usleep(1250 * abs(error));
setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY);
setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY);
}
void PwmSysfsDriverNode::dutyCallback(std_msgs::Float64::ConstPtr const& msg)
{
setDuty(msg->data);
}
void UARTworker(void)
{
unsigned char c,mode=0,addr=0,instruction=0,EEaddrF=0,EEaddr=0,adcc=0,helpC;
initUART();
//write start message (menu)
UARTwriteString(msgMenu[0]);
UARTwrite('\n');
while(1)
{
if(RCIF)
{
RCIF=0;
LED2ON;
if(!(RCSTA&0b00000110))
{ rhead++;
rhead&=RINGBUFFMASK;
ringbuff[rhead]=RCREG;
}
LED2OFF;
c=UARTread();
UARTwrite(c);
//c=UARTcharFromString(c);
switch (mode)
{
case 0:
mode=c-48;
UARTwriteString(msgMenu[c-48]);
if(mode==2)enablePWM();
else if(mode==3)enableDAC();
break;
case 1://ADC
switch(c)
{
case 'r'://single read
UARTwriteString("\n\nADC value: ");
helpC=getADC(adcc);
UARTwriteDecimal(helpC);
UARTwriteString(msgMenu[1]);
break;
case '1'://chanell one
UARTwriteString("\n\nchannel 1 selected");
adcc=0;
UARTwriteString(msgMenu[1]);
break;
case '2'://chanel two
UARTwriteString("\n\nchannel 2 selected");
adcc=1;
UARTwriteString(msgMenu[1]);
break;
case '3'://chanell three
UARTwriteString("\n\nchannel 3 selected");
adcc=2;
UARTwriteString(msgMenu[1]);
break;
case 't'://temp
UARTwriteString("\n\nTemp sensor selected");
adcc=3;
UARTwriteString(msgMenu[1]);
break;
case 'm'://back to start
mode = 0;
UARTwriteString(msgMenu[0]);
break;
default:
break;
}
break;
case 2://PWM
if(instruction)
{
switch(instruction)
{
case 'p':
//pwm period = c;
setPeriod(UARTcharFromString(c));
UARTwriteString(msgMenu[2]);
break;
case 'd':
setDuty(UARTcharFromString(c));
UARTwriteString(msgMenu[2]);
//pwm period =c;
break;
case 'm':
mode =0;
//pwm off
UARTwriteString(msgMenu[0]);
break;
default:
break;
}
instruction = 0;
}
else
{
instruction = c; //loads the instruction
if(instruction == 'p')
{
UARTwriteString("\n\nEnter the PWM Period: ");
}
else if(instruction == 'd')
{
UARTwriteString("\n\nEnter the PWM Duty Cycle: ");
}
else if(instruction == 'm') //if it's m goes back to the start menu...
{
mode =0;
instruction =0;
disablePWM();
UARTwriteString(msgMenu[0]);
}
}
break;
case 3://DAC
if(instruction)
{
switch(instruction)
{
case 'v': //enter woltage
setDAC(UARTcharFromString(c));
UARTwriteString(msgMenu[3]);
break;
case 'm':
mode = 0;
UARTwriteString(msgMenu[0]);
break;
default:
break;
}
instruction =0;
}
else
{
instruction = c; //loads the instruction
if(instruction == 'v')
{
UARTwriteString(msgDACsetV);
}
else if(instruction == 'm') //if it's m goes back to the start menu...
{
mode =0;
instruction =0;
disableDAC();
UARTwriteString(msgMenu[0]);
}
}
break;
case 4://MEM
if(instruction) //if instruction has been sent previusly
{
if(EEaddrF) //instruction was sent previusly, check if address was sent
{ //address was sent
if(instruction == 'w') //if instruction was W-writes recived character to EEProm[ADDR]
{
EEPROMwrite(EEaddr,UARTcharFromString(c));
UARTwriteString(msgMenu[4]);
//write c to eeprom
}
else if (instruction == 'r') //if instruction was R-reads EEprom[addr] from eeprom
{
UARTwriteDecimal(EEPROMread(EEaddr));
UARTwriteString(msgMenu[4]);
}
else if (instruction == 'm') //if instruction was m --returns to start menu...
{
mode = 0;
UARTwriteString(msgMenu[0]);
}
EEaddrF=0; //clears the addressing flag
instruction =0; //clears the istruction flag
}
else
{
EEaddrF=1; //sets the address flage
EEaddr=UARTcharFromString(c);
if(instruction=='w')UARTwriteString(msgEEw);
else if(instruction == 'r')UARTwriteString("\n\nHit any key to read from EEPROM.\n\n");
}
}
else
{
instruction = c; //loads the instruction
if((instruction == 'w')||(instruction == 'r'))
{
UARTwriteString(msgEEaddr);
}
else if(instruction == 'm') //if it's m goes back to the start menu...
{
mode =0;
instruction =0;
UARTwriteString(msgMenu[0]);
}
}
break;
default:
mode=0;
UARTwriteString(msgMenu[0]);
break;
}
}
}
}
void DutyUnit::nr1Change(const unsigned newNr1, const unsigned long cc) {
updatePos(cc);
setDuty(newNr1);
setCounter();
}
/*MAIN*/
int main()
{
//Declarations
int i, j;
int targets;
int center, error, most_blob;
int num_balls = 5;
int dist, prev;
int n = 0;
//Set up GPIO
gpio_export(POWER_LED_PIN);
gpio_set_dir(POWER_LED_PIN, OUTPUT_PIN);
gpio_export(CAMERA_LED_PIN);
gpio_set_dir(CAMERA_LED_PIN, OUTPUT_PIN);
gpio_export(BUTTON_PIN);
gpio_set_dir(BUTTON_PIN, INPUT_PIN);
gpio_export(RELOAD_LED_PIN);
gpio_set_dir(RELOAD_LED_PIN, OUTPUT_PIN);
gpio_export(ARDUINO_PIN);
gpio_set_dir(ARDUINO_PIN, OUTPUT_PIN);
gpio_set_value(ARDUINO_PIN, LOW);
//Set up PWM
initPWM();
usleep(microsecondsToMilliseconds(500));
//Dropper
setPeriod(DROPPER_MOTOR_INDEX, PWM_PERIOD);
setDuty(DROPPER_MOTOR_INDEX, percentageToDuty(DROPPER_DOWN_POSITION));
setRun(DROPPER_MOTOR_INDEX, 1);
//Grabber
setPeriod(GRABBER_MOTOR_INDEX, PWM_PERIOD);
setDuty(GRABBER_MOTOR_INDEX, GRABBER_UP_POSITION);
setRun(GRABBER_MOTOR_INDEX, 1);
//Right wheel
setPeriod(RIGHT_WHEEL_INDEX, PWM_PERIOD);
setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY);
setRun(RIGHT_WHEEL_INDEX, 0);
//Left wheel
setPeriod(LEFT_WHEEL_INDEX, PWM_PERIOD);
setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY);
setRun(LEFT_WHEEL_INDEX, 0);
//Roller
setRun(ROLLER_MOTOR_INDEX, 0);
setPeriod(ROLLER_MOTOR_INDEX, PWM_PERIOD);
setDuty(ROLLER_MOTOR_INDEX, PWM_PERIOD*ROLLER_SPEED);
//Set up serial
serial* arduinoSerial;
if (serial_open(&arduinoSerial, ARDUINO_SERIAL_PORT, ARDUINO_BAUD_RATE) == 0){
printf("Opened Ardunio serial port sucessfully\n");
}
else{
printf("Failed to open Arduino serial port\n");
}
/*for(i = 0; i < 50; i++)
{
dist = getDistance(arduinoSerial);
printf("Instensity: %d\n", dist);
usleep(microsecondsToMilliseconds(100));
}
usleep(microsecondsToMilliseconds(5000));*/
//Turn on power LED
gpio_set_value(POWER_LED_PIN, HIGH);
printf("Systems are GO for launch!\n");
waitForButtonPress();
getDistance(arduinoSerial);
//Move to center of the board
goForward();
usleep(microsecondsToMilliseconds(1800));
stopWheels();
//Aim at right target
rotate(180);
if(aimAtSideMostTarget(1))
{
for (; num_balls > 3; num_balls--)
{
fireGolfBall();
}
}
//Move toward left target
rotate(-550);
goForward();
usleep(microsecondsToMilliseconds(2200));
stopWheels();
//Aim at left target
rotate(300);
if(aimAtSideMostTarget(0))
{
for (; num_balls > 1; num_balls--)
{
fireGolfBall();
}
}
//Aim at middle target
rotate(260);
aimAtBiggestTarget();
for(; num_balls > 0; num_balls--)
{
fireGolfBall();
}
//Move to sideline for reload
rotate(-600);
goForward();
buffer_clear(arduinoSerial);
do
{
prev = dist;
dist = getDistance(arduinoSerial);
//printf("%d, ", dist);
usleep(microsecondsToMilliseconds(9));
}
while (dist < DISTANCE_TO_WALL || dist == 0 || prev - dist > 5);
stopWheels();
rotate(1100);
//Turn on reload LED
gpio_set_value(RELOAD_LED_PIN, HIGH);
usleep(microsecondsToMilliseconds(3000));
gpio_set_value(RELOAD_LED_PIN, LOW);
while (n < 3) //big while loop thing!
{
//start from left
num_balls = 5;
goForward();
usleep(microsecondsToMilliseconds(1200));
stopWheels();
rotate(-420);
goForward();
usleep(microsecondsToMilliseconds(590));
stopWheels();
//Aim at left target
if(aimAtSideMostTarget(0))
{
for(; num_balls > 3; num_balls--)
{
fireGolfBall();
}
}
//Aim at middle target
rotate(420);
goForward();
usleep(microsecondsToMilliseconds(600));
stopWheels();
rotate(-150);
aimAtBiggestTarget();
for(; num_balls > 2; num_balls--)
{
fireGolfBall();
}
//Aim at right target
rotate(400);
goForward();
usleep(microsecondsToMilliseconds(2700));
stopWheels();
rotate(-370);
if(aimAtSideMostTarget(1))
{
for(; num_balls > 0; num_balls--)
{
fireGolfBall();
}
}
//Move to sideline for reload
rotate(320);
//usleep(microsecondsToMilliseconds(50));
goForward();
buffer_clear(arduinoSerial);
dist = 0;
do
{
prev = dist;
dist = getDistance(arduinoSerial);
//printf("And another thing....\n");
//printf("%d!!! \n", dist);
usleep(microsecondsToMilliseconds(10));
}while (dist < DISTANCE_TO_WALL || dist == 0 || prev - dist > 5);
stopWheels();
rotate(-900);
gpio_set_value(RELOAD_LED_PIN, HIGH);
usleep(microsecondsToMilliseconds(3000));
gpio_set_value(RELOAD_LED_PIN, LOW);
//start from right
num_balls = 5;
goForward();
usleep(microsecondsToMilliseconds(1350));
stopWheels();
rotate(570);
//Aim at right target
if(aimAtSideMostTarget(1))
{
for(; num_balls > 3; num_balls--)
{
fireGolfBall();
}
}
//Aim at middle target
rotate(-500);
goForward();
usleep(microsecondsToMilliseconds(4500));
stopWheels();
rotate(650);
aimAtBiggestTarget();
for(; num_balls > 2; num_balls--)
{
fireGolfBall();
}
//Aim at left target
rotate(-200);
if(aimAtSideMostTarget(0))
{
for(; num_balls > 0; num_balls--)
{
fireGolfBall();
}
}
//Move to sideline for reload
rotate(-400);
goForward();
buffer_clear(arduinoSerial);
do
{
prev = dist;
dist = getDistance(arduinoSerial);
usleep(microsecondsToMilliseconds(9));
}
while(dist < DISTANCE_TO_WALL || dist == 0 || prev - dist > 5);
stopWheels();
rotate(1000);
gpio_set_value(RELOAD_LED_PIN, HIGH);
usleep(microsecondsToMilliseconds(3000));
gpio_set_value(RELOAD_LED_PIN, LOW);
} //end of while loop
//Turn off gpio power
gpio_set_value(POWER_LED_PIN, LOW);
gpio_set_value(RELOAD_LED_PIN, LOW);
gpio_set_value(ARDUINO_PIN, LOW);
//Turn off motors
setRun(GRABBER_MOTOR_INDEX, 0);
setRun(DROPPER_MOTOR_INDEX, 0);
setRun(ROLLER_MOTOR_INDEX, 0);
setRun(RIGHT_WHEEL_INDEX, 0);
setRun(LEFT_WHEEL_INDEX, 0);
//Remove GPIO files
//gpio_unexport(PING_PIN);
return 0;
}
void goForward(void){
setRun(LEFT_WHEEL_INDEX, 1);
setRun(RIGHT_WHEEL_INDEX, 1);
setDuty(LEFT_WHEEL_INDEX, MAXIMUM_DUTY);
setDuty(RIGHT_WHEEL_INDEX, MINIMUM_DUTY);
}
void stopWheels(void){
setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY);
setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY);
}
void liftGolfBall(void){
setDuty(GRABBER_MOTOR_INDEX, GRABBER_UP_POSITION);
usleep(microsecondsToMilliseconds(700));
setDuty(GRABBER_MOTOR_INDEX, GRABBER_DOWN_POSITION);
usleep(microsecondsToMilliseconds(500));
}
void dropGolfBall(void){
setDuty(DROPPER_MOTOR_INDEX, percentageToDuty(DROPPER_UP_POSITION));
usleep(microsecondsToMilliseconds(400));
setDuty(DROPPER_MOTOR_INDEX, percentageToDuty(DROPPER_DOWN_POSITION));
usleep(microsecondsToMilliseconds(250));
}
