void deactivateMotorShield()
{
const uint8_t BUFFER_LEN = 32;
char buffer[BUFFER_LEN];
ROS_INFO("Shutting down motor-shield...\n");
motors_control(0,0,'A');
motors_control(0,0,'B');
//Unregsiter PWM module
snprintf(buffer,BUFFER_LEN, "%d",BRAKE_A_IO);
sys_write("/sys/class/gpio/unexport", buffer);
snprintf(buffer,BUFFER_LEN, "%d",BRAKE_B_IO);
sys_write("/sys/class/gpio/unexport", buffer);
snprintf(buffer,BUFFER_LEN, "%d",DIR_A_IO);
sys_write("/sys/class/gpio/unexport", buffer);
snprintf(buffer,BUFFER_LEN, "%d",DIR_B_IO);
sys_write("/sys/class/gpio/unexport", buffer);
snprintf(buffer,BUFFER_LEN, "%d",PWM_A);
sys_write("/sys/class/pwm/pwmchip0/unexport", buffer);
snprintf(buffer,BUFFER_LEN, "%d",PWM_B);
sys_write("/sys/class/pwm/pwmchip0/unexport", buffer);
//return 1;
}
/*
* Runs the scheduler.
*/
__inline void sched_run (void)
{
uint8_t flags;
/* Atomic store temporary flags and reset real flags in case we run a bit late. */
cli();
flags = sched_flags;
sched_flags = 0;
sei();
/*
* Run tasks.
*
* Very important that the periodicity of the scheduler update task
* allows this to finish or the loss of task execution may occur.
*/
/* Motor task. */
if (flags & SCHED_MOTORS)
motors_control();
#if MOSKI_USE_TEMP
/* Temp task. */
if (flags & SCHED_TEMP)
temp_start(); /* Start temperature conversion. */
#endif /* MOSKI_USE_TEMP */
}
void activateMotorShield()
{
const uint8_t BUFFER_LEN = 32;
char buffer[BUFFER_LEN];
//Deactivate VDD_SHIELD_SW by asserting TRISTATE_ALL
snprintf(buffer,BUFFER_LEN, "%d",TRISTATE_ALL);
sys_write("/sys/class/gpio/export", buffer);
sys_write("/sys/class/gpio/gpio%d/direction",TRISTATE_ALL ,"out");
sys_write("/sys/class/gpio/gpio%d/value",TRISTATE_ALL, "0");
sys_write("/sys/class/gpio/unexport",buffer);
//Configure Brake A
snprintf(buffer,BUFFER_LEN, "%d",BRAKE_A_LS);
sys_write("/sys/class/gpio/export", buffer);
sys_write("/sys/class/gpio/gpio%d/direction",BRAKE_A_LS ,"out");
sys_write("/sys/class/gpio/gpio%d/value",BRAKE_A_LS, "1");
sys_write("/sys/class/gpio/unexport",buffer);
snprintf(buffer,BUFFER_LEN, "%d",BRAKE_A_IO);
sys_write("/sys/class/gpio/export", buffer);
sys_write("/sys/class/gpio/gpio%d/direction",BRAKE_A_IO ,"out");
sys_write("/sys/class/gpio/gpio%d/value",BRAKE_A_IO, "1");
//Configure Brake B
snprintf(buffer,BUFFER_LEN, "%d",BRAKE_B_LS);
sys_write("/sys/class/gpio/export", buffer);
sys_write("/sys/class/gpio/gpio%d/direction",BRAKE_B_LS ,"out");
sys_write("/sys/class/gpio/gpio%d/value",BRAKE_B_LS, "1");
sys_write("/sys/class/gpio/unexport",buffer);
snprintf(buffer,BUFFER_LEN, "%d",BRAKE_B_IO);
sys_write("/sys/class/gpio/export", buffer);
sys_write("/sys/class/gpio/gpio%d/direction",BRAKE_B_IO ,"out");
sys_write("/sys/class/gpio/gpio%d/value",BRAKE_B_IO, "1");
//Configure Dir A
snprintf(buffer,BUFFER_LEN, "%d",DIR_A_LS);
sys_write("/sys/class/gpio/export", buffer);
sys_write("/sys/class/gpio/gpio%d/direction",DIR_A_LS ,"out");
sys_write("/sys/class/gpio/gpio%d/value",DIR_A_LS, "1");
sys_write("/sys/class/gpio/unexport",buffer);
sys_write("/sys/class/gpio/export",DIR_A_MUX);
sys_write("/sys/class/gpio/gpio%d/direction",DIR_A_MUX ,"out");
sys_write("/sys/class/gpio/gpio%d/value",DIR_A_MUX, "0");
sys_write("/sys/class/gpio/unexport", DIR_A_MUX);
sys_write("/sys/kernel/debug/gpio_debug/gpio%d/current_pinmux", DIR_A_IO, "mode0");
snprintf(buffer,BUFFER_LEN, "%d",DIR_A_IO);
sys_write("/sys/class/gpio/export", buffer);
sys_write("/sys/class/gpio/gpio%d/direction",DIR_A_IO ,"out");
sys_write("/sys/class/gpio/gpio%d/value",DIR_A_IO, "0");
//Configure Dir B
snprintf(buffer,BUFFER_LEN, "%d",DIR_B_LS);
sys_write("/sys/class/gpio/export", buffer);
sys_write("/sys/class/gpio/gpio%d/direction",DIR_B_LS ,"out");
sys_write("/sys/class/gpio/gpio%d/value",DIR_B_LS, "1");
sys_write("/sys/class/gpio/unexport",buffer);
sys_write("/sys/class/gpio/export",DIR_B_MUX);
sys_write("/sys/class/gpio/gpio%d/direction",DIR_B_MUX ,"out");
sys_write("/sys/class/gpio/gpio%d/value",DIR_B_MUX, "0");
sys_write("/sys/class/gpio/unexport", DIR_B_MUX);
sys_write("/sys/kernel/debug/gpio_debug/gpio%d/current_pinmux", DIR_B_IO, "mode0");
snprintf(buffer,BUFFER_LEN, "%d",DIR_B_IO);
sys_write("/sys/class/gpio/export", buffer);
sys_write("/sys/class/gpio/gpio%d/direction",DIR_B_IO ,"out");
sys_write("/sys/class/gpio/gpio%d/value",DIR_B_IO, "0");
//Activate PWM A
snprintf(buffer,BUFFER_LEN, "%d",PWM_A_LS);
sys_write("/sys/class/gpio/export", buffer);
sys_write("/sys/class/gpio/gpio%d/direction",PWM_A_LS ,"out");
sys_write("/sys/class/gpio/gpio%d/value",PWM_A_LS, "1");
sys_write("/sys/class/gpio/unexport",buffer);
snprintf(buffer,BUFFER_LEN, "%d",PWM_A_IO);
sys_write("/sys/class/gpio/export", buffer);
sys_write("/sys/class/gpio/gpio%d/direction",PWM_A_IO ,"out");
sys_write("/sys/class/gpio/gpio%d/value",PWM_A_IO, "0");
sys_write("/sys/class/gpio/unexport",buffer);
snprintf(buffer,BUFFER_LEN, "%d",PWM_A);
sys_write("/sys/class/pwm/pwmchip0/export", buffer);
sys_write("/sys/class/pwm/pwmchip0/pwm%d/enable",PWM_A ,"1");
snprintf(buffer,BUFFER_LEN, "%d",PWM_PERIOD);
sys_write("/sys/class/pwm/pwmchip0/pwm%d/period",PWM_A, buffer);
sys_write("/sys/class/pwm/pwmchip0/pwm%d/duty_cycle", PWM_A, "0");
sys_write("/sys/kernel/debug/gpio_debug/gpio%d/current_pinmux", PWM_A_IO, "mode1");
//Activate PWM B
snprintf(buffer,BUFFER_LEN, "%d",PWM_B_LS);
sys_write("/sys/class/gpio/export", buffer);
sys_write("/sys/class/gpio/gpio%d/direction",PWM_B_LS ,"out");
sys_write("/sys/class/gpio/gpio%d/value",PWM_B_LS, "1");
sys_write("/sys/class/gpio/unexport",buffer);
snprintf(buffer,BUFFER_LEN, "%d",PWM_B_IO);
sys_write("/sys/class/gpio/export", buffer);
sys_write("/sys/class/gpio/gpio%d/direction",PWM_B_IO ,"out");
sys_write("/sys/class/gpio/gpio%d/value",PWM_B_IO, "0");
sys_write("/sys/class/gpio/unexport",buffer);
snprintf(buffer,BUFFER_LEN, "%d",PWM_B);
sys_write("/sys/class/pwm/pwmchip0/export", buffer);
sys_write("/sys/class/pwm/pwmchip0/pwm%d/enable",PWM_B ,"1");
snprintf(buffer,BUFFER_LEN, "%d",PWM_PERIOD);
sys_write("/sys/class/pwm/pwmchip0/pwm%d/period",PWM_B, buffer);
sys_write("/sys/class/pwm/pwmchip0/pwm%d/duty_cycle", PWM_B, "0");
sys_write("/sys/kernel/debug/gpio_debug/gpio%d/current_pinmux", PWM_B_IO, "mode1");
//Activate VDD_SHIELD_SW by asserting TRISTATE_ALL
snprintf(buffer,BUFFER_LEN, "%d",TRISTATE_ALL);
sys_write("/sys/class/gpio/export", buffer);
sys_write("/sys/class/gpio/gpio%d/direction",TRISTATE_ALL ,"out");
sys_write("/sys/class/gpio/gpio%d/value",TRISTATE_ALL, "1");
sys_write("/sys/class/gpio/unexport",buffer);
ROS_INFO("All GPIOs and PWMs set...");
motors_control(0,0,'A');
motors_control(0,0,'B');
}
