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'); }