void update_motor(motor_index_t index, motor_direction_t direction, uint16_t duty_tenths_perc){
GPIO_write(STBY_MOTOR, ON);
switch(direction){
case CW:
// Set the pins so that the direction is Clockwise
GPIO_write(motors[index].dir_pins[0], ON);
GPIO_write(motors[index].dir_pins[1], OFF);
break;
case CCW:
// Set the pins so that the direction is Counter-Clockwise
GPIO_write(motors[index].dir_pins[0], OFF);
GPIO_write(motors[index].dir_pins[1], ON);
break;
case BRAKE:
// Set the pins so that we brake
GPIO_write(motors[index].dir_pins[0], ON);
GPIO_write(motors[index].dir_pins[1], ON);
break;
case STOP:
// Set the pins so we stop
GPIO_write(motors[index].dir_pins[0], OFF);
GPIO_write(motors[index].dir_pins[1], OFF);
break;
default:
break;
}
uint32_t usec = MOTOR_PERIOD*duty_tenths_perc/1000;
set_pulse_width(index, &usec);
}
int main(int argc, char **argv) {
int rtn;
if (argc != 2)
usage();
/* prussdrv_init() will segfault if called with EUID != 0 */
if(geteuid()) {
fprintf(stderr, "%s must be run as root to use prussdrv\n", argv[0]);
return -1;
}
/* initialize the library, PRU and interrupt; launch our PRU program */
if(pru_setup(argv[1])) {
pru_cleanup();
return -1;
}
int pulse_width;
for (pulse_width = 0; pulse_width <= 1000; pulse_width += 50) {
set_pulse_width(pulse_width);
sleep_tenths(1);
}
/* wait for PRU to assert the interrupt to indicate completion */
printf("waiting for interrupt from PRU0...\n");
/* The prussdrv_pru_wait_event() function returns the number of times
the event has taken place, as an unsigned int. There is no out-of-
band value to indicate error (and it can wrap around to 0 if you
run the program just a whole lot of times). */
rtn = prussdrv_pru_wait_event(PRU_EVTOUT_0);
printf("PRU program completed, event number %d\n", rtn);
//int *pruDataMem_int = (int*)pruDataMem;
//printf("Contents of PRU DATA RAM: %08x\n", pruDataMem[0]);
printf("Contents of PRU DATA RAM: %08x\n", pruDataMem->hi_delay);
/* clear the event, disable the PRU and let the library clean up */
return pru_cleanup();
}
static int
angle_set(struct sol_flow_node *node, void *data, uint16_t port, uint16_t conn_id, const struct sol_flow_packet *packet)
{
double in_value;
struct servo_motor_data *mdata = data;
int r, pulse_width;
r = sol_flow_packet_get_drange_value(packet, &in_value);
SOL_INT_CHECK(r, < 0, r);
if (isless(in_value, 0) || isgreaterequal(in_value, 180)) {
SOL_WRN("Invalid value %f. It must be >= 0 and < 180", in_value);
return -EINVAL;
}
pulse_width = in_value * mdata->duty_cycle_diff / 180 +
mdata->duty_cycle_range.min;
return set_pulse_width(mdata, pulse_width);
}
static int
duty_cycle_set(struct sol_flow_node *node, void *data, uint16_t port, uint16_t conn_id, const struct sol_flow_packet *packet)
{
struct servo_motor_data *mdata = data;
int r;
int32_t in_value;
r = sol_flow_packet_get_irange_value(packet, &in_value);
SOL_INT_CHECK(r, < 0, r);
if (in_value < mdata->duty_cycle_range.min ||
in_value > mdata->duty_cycle_range.max) {
SOL_WRN("Invalid value %" PRId32 "."
"It must be >= %" PRId32 " and =< %" PRId32 "", in_value,
mdata->duty_cycle_range.min, mdata->duty_cycle_range.max);
return -EINVAL;
}
return set_pulse_width(mdata, in_value);
}
