159 lines
5.5 KiB
C
159 lines
5.5 KiB
C
/* Methods for setting up the i2c channel to the
|
|
* motor board and some methods for initializing
|
|
* running the motors. i2c setup uses wiringPi library.
|
|
* Methods and header definitions from a RPI forum thread,
|
|
* specifically, the post by garagebrewer:
|
|
* https://www.raspberrypi.org/forums/viewtopic.php?t=112415&p=770234
|
|
*/
|
|
#include <wiringPiI2C.h>
|
|
#include <stdio.h>
|
|
#include "motorHat.h"
|
|
|
|
void setAllPWM(word i2c, word on, word off){
|
|
wiringPiI2CWriteReg8(i2c, PWM_ALL_LED_ON_L, on & 0xFF);
|
|
wiringPiI2CWriteReg8(i2c, PWM_ALL_LED_ON_H, on >> 8);
|
|
wiringPiI2CWriteReg8(i2c, PWM_ALL_LED_OFF_L, off & 0xFF);
|
|
wiringPiI2CWriteReg8(i2c, PWM_ALL_LED_OFF_H, off >> 8);
|
|
}
|
|
|
|
void setPWM(word i2c, word pin, word on, word off){
|
|
wiringPiI2CWriteReg8(i2c, PWM_LED0_ON_L + 4 * pin, on & 0xFF);
|
|
wiringPiI2CWriteReg8(i2c, PWM_LED0_ON_H + 4 * pin, on >> 8);
|
|
wiringPiI2CWriteReg8(i2c, PWM_LED0_OFF_L + 4 * pin, off & 0xFF);
|
|
wiringPiI2CWriteReg8(i2c, PWM_LED0_OFF_H + 4 * pin, off >> 8);
|
|
}
|
|
|
|
void setPin(word i2c, word pin, word value){
|
|
if(pin < 0 || pin > 15){
|
|
printf("PWM pin must be between 0 and 15 inclusive. Received '%d'\n", pin);
|
|
return;
|
|
}
|
|
|
|
switch(value){
|
|
case 0:
|
|
setPWM(i2c, pin, 0, 4096);
|
|
break;
|
|
case 1:
|
|
setPWM(i2c, pin, 4096, 0);
|
|
break;
|
|
default:
|
|
printf("PWM pin value must be 0 or 1. Received '%d'\n", pin);
|
|
return;
|
|
}
|
|
}
|
|
|
|
void runMotor(word i2c, word motor, word command){
|
|
word in1, in2, in3, in4;
|
|
|
|
switch(motor){
|
|
case 1:
|
|
in1 = PWM_M1_IN1;
|
|
in2 = PWM_M1_IN2;
|
|
break;
|
|
case 2:
|
|
in1 = PWM_M2_IN1;
|
|
in2 = PWM_M2_IN2;
|
|
break;
|
|
case 3:
|
|
in1 = PWM_M3_IN1;
|
|
in2 = PWM_M3_IN2;
|
|
break;
|
|
case 4:
|
|
in1 = PWM_M4_IN1;
|
|
in2 = PWM_M4_IN2;
|
|
break;
|
|
case 12:
|
|
case 21:
|
|
in1 = PWM_M1_IN1;
|
|
in2 = PWM_M1_IN2;
|
|
in3 = PWM_M2_IN1;
|
|
in4 = PWM_M2_IN2;
|
|
break;
|
|
default:
|
|
printf("Invalid motor number '%d'\n", motor);
|
|
return;
|
|
}
|
|
|
|
switch(command){
|
|
case MOTOR_FORWARD:
|
|
setPin(i2c, in2, 0);
|
|
setPin(i2c, in1, 1);
|
|
setPin(i2c, in4, 0);
|
|
setPin(i2c, in3, 1);
|
|
break;
|
|
case MOTOR_BACK:
|
|
setPin(i2c, in1, 0);
|
|
setPin(i2c, in2, 1);
|
|
setPin(i2c, in4, 0);
|
|
setPin(i2c, in3, 1);
|
|
break;
|
|
case MOTOR_RIGHT:
|
|
setPin(i2c, in1, 1);
|
|
setPin(i2c, in2, 0);
|
|
setPin(i2c, in3, 0);
|
|
setPin(i2c, in4, 0);
|
|
break;
|
|
case MOTOR_LEFT:
|
|
setPin(i2c, in1, 0);
|
|
setPin(i2c, in2, 0);
|
|
setPin(i2c, in3, 1);
|
|
setPin(i2c, in4, 0);
|
|
break;
|
|
case MOTOR_PIVOT_RIGHT:
|
|
setPin(i2c, in1, 1);
|
|
setPin(i2c, in2, 0);
|
|
setPin(i2c, in3, 0);
|
|
setPin(i2c, in4, 1);
|
|
break;
|
|
case MOTOR_PIVOT_LEFT:
|
|
setPin(i2c, in1, 0);
|
|
setPin(i2c, in2, 1);
|
|
setPin(i2c, in3, 1);
|
|
setPin(i2c, in4, 0);
|
|
break;
|
|
case MOTOR_RELEASE:
|
|
setPin(i2c, in1, 0);
|
|
setPin(i2c, in2, 0);
|
|
setPin(i2c, in4, 0);
|
|
setPin(i2c, in3, 0);
|
|
break;
|
|
default:
|
|
printf("Unsupported command '%d'\n", command);
|
|
return;
|
|
}
|
|
}
|
|
|
|
void setSpeed(word i2c, word motor, word speed){
|
|
if(speed < 0 || speed > 255){
|
|
printf("Speed must be between 0 and 255 inclusive. Received '%d'\n", speed);
|
|
return;
|
|
}
|
|
|
|
word pwm;
|
|
switch(motor){
|
|
case 1:
|
|
pwm = PWM_M1_PWM;
|
|
break;
|
|
case 2:
|
|
pwm = PWM_M2_PWM;
|
|
break;
|
|
case 3:
|
|
pwm = PWM_M3_PWM;
|
|
break;
|
|
case 4:
|
|
pwm = PWM_M4_PWM;
|
|
break;
|
|
default:
|
|
printf("Unsupported motor '%s'\n", motor);
|
|
break;
|
|
}
|
|
setPWM(i2c, pwm, 0, speed * 16);
|
|
}
|
|
|
|
void initMotor(word i2c, word motor){
|
|
runMotor(i2c, motor, MOTOR_RELEASE);
|
|
setSpeed(i2c, motor, 150);
|
|
runMotor(i2c, motor, MOTOR_FORWARD);
|
|
runMotor(i2c, motor, MOTOR_RELEASE);
|
|
}
|