48 lines
1.0 KiB
C
48 lines
1.0 KiB
C
#include "headers/motors.h"
|
|
|
|
#include <pico/types.h>
|
|
#include "i2c/headers/mcp23017.h"
|
|
|
|
#define MOTOR1_DIR1_MCP23017_GPIO 12
|
|
#define MOTOR1_DIR2_MCP23017_GPIO 13
|
|
#define MOTOR2_DIR1_MCP23017_GPIO 14
|
|
#define MOTOR2_DIR2_MCP23017_GPIO 15
|
|
|
|
void motor_set_dir(motors_t motor, int16_t value)
|
|
{
|
|
uint gpio_dir1_pin;
|
|
uint gpio_dir2_pin;
|
|
|
|
switch(motor)
|
|
{
|
|
case MOTOR1:
|
|
gpio_dir1_pin = MOTOR1_DIR1_MCP23017_GPIO;
|
|
gpio_dir2_pin = MOTOR1_DIR2_MCP23017_GPIO;
|
|
break;
|
|
|
|
case MOTOR2:
|
|
gpio_dir1_pin = MOTOR2_DIR1_MCP23017_GPIO;
|
|
gpio_dir2_pin = MOTOR2_DIR2_MCP23017_GPIO;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
if(value > 0)
|
|
{
|
|
mcp23017_gpio_put(gpio_dir1_pin, false);
|
|
mcp23017_gpio_put(gpio_dir2_pin, true);
|
|
}
|
|
else if(value < 0)
|
|
{
|
|
mcp23017_gpio_put(gpio_dir1_pin, true);
|
|
mcp23017_gpio_put(gpio_dir2_pin, false);
|
|
}
|
|
else
|
|
{
|
|
mcp23017_gpio_put(gpio_dir1_pin, false);
|
|
mcp23017_gpio_put(gpio_dir2_pin, false);
|
|
}
|
|
}
|