mirror of
https://github.com/OSURoboticsClub/Rover_2017_2018.git
synced 2025-11-08 10:11:14 +00:00
Mining system completegit add .!
This commit is contained in:
@@ -1,289 +1,335 @@
|
||||
////////// Includes //////////
|
||||
#include "HX711.h"
|
||||
|
||||
////////// Hardware / Data Enumerations //////////
|
||||
enum HARDWARE {
|
||||
RS485_EN = 6,
|
||||
RS485_RX = 9,
|
||||
RS485_TX = 10,
|
||||
|
||||
MOTOR_LIFT_A = 27,
|
||||
MOTOR_LIFT_B = 28,
|
||||
MOTOR_LIFT_PWM = 25,
|
||||
MOTOR_LIFT_CS = 31,
|
||||
MOTOR_LIFT_EN = 24,
|
||||
MOTOR_LIFT_FB = A10,
|
||||
|
||||
MOTOR_TILT_A = 30,
|
||||
MOTOR_TILT_B = 29,
|
||||
MOTOR_TILT_PWM = 32,
|
||||
MOTOR_TILT_CS = 26,
|
||||
MOTOR_TILT_EN = 33,
|
||||
MOTOR_TILT_FB = A11,
|
||||
|
||||
LED_13 = 13,
|
||||
|
||||
LED_RED = 20,
|
||||
LED_BLUE = 21,
|
||||
LED_GREEN = 22,
|
||||
|
||||
SCALE_DOUT = 8,
|
||||
SCALE_CLK = 7
|
||||
|
||||
};
|
||||
|
||||
enum MOTORS {
|
||||
LIFT = 0,
|
||||
TILT = 1,
|
||||
};
|
||||
|
||||
enum MODBUS_REGISTERS {
|
||||
// Inputs
|
||||
SET_POSITION_LIFT = 0,
|
||||
SET_POSITION_TILT = 1,
|
||||
TARE = 2,
|
||||
CALIBRATION_FACTOR = 3,
|
||||
|
||||
// Outputs
|
||||
CURRENT_POSITION_LIFT = 4,
|
||||
CURRENT_POSITION_TILT = 5,
|
||||
MEASURED_WEIGHT = 6,
|
||||
};
|
||||
|
||||
////////// Global Variables //////////
|
||||
int set_position_lift = 0;
|
||||
int set_position_tilt = 0;
|
||||
int tolerance = 20; //tolerance for position
|
||||
|
||||
float calibration_factor = -120000; //for the scale
|
||||
|
||||
// modbus stuff
|
||||
const uint8_t node_id = 2;
|
||||
const uint8_t mobus_serial_port_number = 3;
|
||||
uint16_t modbus_data[] = {0, 0, 0, 0, 0, 0, 0};
|
||||
uint8_t num_modbus_registers = 0;
|
||||
int8_t poll_state = 0;
|
||||
bool communication_good = false;
|
||||
uint8_t message_count = 0;
|
||||
|
||||
// nice human words for motor states
|
||||
#define BRAKEVCC 0
|
||||
#define CW 1
|
||||
#define CCW 2
|
||||
#define BRAKEGND 3
|
||||
|
||||
////////// Class Instantiations //////////
|
||||
Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN);
|
||||
|
||||
void setup() {
|
||||
Serial.begin(9600); // debug
|
||||
|
||||
setup_hardware();
|
||||
num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
|
||||
slave.begin(115200); // baud-rate at 19200
|
||||
slave.setTimeOut(150);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
poll_modbus();
|
||||
set_leds();
|
||||
set_motors();
|
||||
set_scale();
|
||||
poll_scale();
|
||||
}
|
||||
|
||||
void setup_hardware() {
|
||||
pinMode(HARDWARE::RS485_EN, OUTPUT);
|
||||
|
||||
pinMode(HARDWARE::MOTOR_LIFT_A, OUTPUT);
|
||||
pinMode(HARDWARE::MOTOR_LIFT_B, OUTPUT);
|
||||
pinMode(HARDWARE::MOTOR_LIFT_PWM, OUTPUT);
|
||||
pinMode(HARDWARE::MOTOR_LIFT_EN, OUTPUT);
|
||||
pinMode(HARDWARE::MOTOR_LIFT_CS, INPUT);
|
||||
pinMode(HARDWARE::MOTOR_LIFT_FB, INPUT);
|
||||
|
||||
pinMode(HARDWARE::MOTOR_TILT_A, OUTPUT);
|
||||
pinMode(HARDWARE::MOTOR_TILT_B, OUTPUT);
|
||||
pinMode(HARDWARE::MOTOR_TILT_PWM, OUTPUT);
|
||||
pinMode(HARDWARE::MOTOR_TILT_EN, OUTPUT);
|
||||
pinMode(HARDWARE::MOTOR_TILT_CS, INPUT);
|
||||
pinMode(HARDWARE::MOTOR_TILT_FB, INPUT);
|
||||
|
||||
pinMode(HARDWARE::LED_13, OUTPUT);
|
||||
pinMode(HARDWARE::LED_RED, OUTPUT);
|
||||
pinMode(HARDWARE::LED_BLUE, OUTPUT);
|
||||
pinMode(HARDWARE::LED_GREEN, OUTPUT);
|
||||
|
||||
// set defualt states
|
||||
digitalWrite(HARDWARE::LED_RED, LOW);
|
||||
digitalWrite(HARDWARE::LED_GREEN, HIGH);
|
||||
digitalWrite(HARDWARE::LED_BLUE, HIGH);
|
||||
digitalWrite(HARDWARE::LED_13, LOW);
|
||||
digitalWrite(HARDWARE::MOTOR_LIFT_EN, HIGH);
|
||||
digitalWrite(HARDWARE::MOTOR_TILT_EN, HIGH);
|
||||
|
||||
// Change motor PWM frequency so it's not in the audible range
|
||||
analogWriteFrequency(HARDWARE::MOTOR_LIFT_PWM, 25000);
|
||||
analogWriteFrequency(HARDWARE::MOTOR_TILT_PWM, 25000);
|
||||
|
||||
// set the current desired position to the current position
|
||||
set_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB);
|
||||
set_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB);
|
||||
|
||||
// setup scale
|
||||
scale.set_scale();
|
||||
scale.tare(); //Reset the scale to 0
|
||||
scale.set_scale(calibration_factor);
|
||||
}
|
||||
|
||||
void poll_modbus(){
|
||||
poll_state = slave.poll(modbus_data, num_modbus_registers);
|
||||
communication_good = !slave.getTimeOutState();
|
||||
}
|
||||
|
||||
void set_leds(){
|
||||
if(poll_state > 4){
|
||||
message_count++;
|
||||
if(message_count > 2){
|
||||
digitalWrite(HARDWARE::LED_13, !digitalRead(HARDWARE::LED_13));
|
||||
message_count = 0;
|
||||
}
|
||||
|
||||
digitalWrite(HARDWARE::LED_GREEN, LOW);
|
||||
digitalWrite(HARDWARE::LED_RED, HIGH);
|
||||
}else if(!communication_good){
|
||||
digitalWrite(HARDWARE::LED_13, LOW);
|
||||
digitalWrite(HARDWARE::LED_GREEN, HIGH);
|
||||
digitalWrite(HARDWARE::LED_RED, LOW);
|
||||
}
|
||||
}
|
||||
|
||||
void set_motors() {
|
||||
set_position_lift = modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT];
|
||||
set_position_tilt = modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT];
|
||||
|
||||
current_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB);
|
||||
current_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB);
|
||||
|
||||
if (abs(current_position_lift - set_position_lift) > tolerance) {
|
||||
if (current_position_lift < set_position_lift) {
|
||||
set_motor_output(MOTORS::LIFT, CCW, 255);
|
||||
}
|
||||
else {
|
||||
set_motor_output(MOTORS::LIFT, CW, 255);
|
||||
}
|
||||
}
|
||||
else {
|
||||
motor_off(MOTORS::LIFT);
|
||||
}
|
||||
|
||||
if (abs(current_position_tilt - set_position_tilt) > tolerance) {
|
||||
if (current_position_tilt < set_position_tilt) {
|
||||
set_motor_output(MOTORS::TILT, CCW, 255);
|
||||
}
|
||||
else {
|
||||
set_motor_output(MOTORS::TILT, CW, 255);
|
||||
}
|
||||
}
|
||||
else {
|
||||
motor_off(MOTORS::TILT);
|
||||
}
|
||||
}
|
||||
|
||||
void set_scale(){
|
||||
scale.set_scale(modbus_data[MODBUS_REGISTERS::CALIBRATION_FACTOR]);
|
||||
}
|
||||
|
||||
void poll_scale(){
|
||||
modbus_data[MODBUS_REGISTERS::MEASURED_WEIGHT] = scale.get_units()*-1
|
||||
}
|
||||
|
||||
//---Set Motor Output---//
|
||||
/*
|
||||
Inputs: motor number, direction, pwm value
|
||||
Returns: nothing
|
||||
|
||||
Will set a motor going in a specific direction the motor will continue
|
||||
going in that direction, at that speed until told to do otherwise.
|
||||
|
||||
direct: Should be between 0 and 3, with the following result
|
||||
0: Brake to VCC
|
||||
1: Clockwise
|
||||
2: CounterClockwise
|
||||
3: Brake to GND
|
||||
|
||||
pwm: should be a value between 0 and 255, higher the number, the faster
|
||||
it'll go
|
||||
----------------
|
||||
Control Logic:
|
||||
----------------
|
||||
A | B
|
||||
Brake VCC: 1 1
|
||||
CW: 1 0
|
||||
CCW: 0 1
|
||||
Brake GND: 0 0
|
||||
----------------
|
||||
*/
|
||||
void set_motor_output(int motor, int direction, int pwm_input) {
|
||||
|
||||
int a;
|
||||
int b;
|
||||
int pwm;
|
||||
|
||||
if (motor == MOTORS::LIFT) {
|
||||
a = HARDWARE::MOTOR_LIFT_A;
|
||||
b = HARDWARE::MOTOR_LIFT_B;
|
||||
pwm = HARDWARE::MOTOR_LIFT_PWM;
|
||||
}
|
||||
else if (motor == MOTORS::TILT) {
|
||||
a = HARDWARE::MOTOR_TILT_A;
|
||||
b = HARDWARE::MOTOR_TILT_B;
|
||||
pwm = HARDWARE::MOTOR_TILT_PWM;
|
||||
}
|
||||
else {
|
||||
return;
|
||||
}
|
||||
|
||||
if (direction <= 4) {
|
||||
// Set A
|
||||
if (direction <= 1) {
|
||||
digitalWrite(a, HIGH);
|
||||
}
|
||||
else {
|
||||
digitalWrite(a, LOW);
|
||||
}
|
||||
|
||||
// Set B
|
||||
if ((direction == 0) || (direction == 2)) {
|
||||
digitalWrite(b, HIGH);
|
||||
}
|
||||
else {
|
||||
digitalWrite(b, LOW);
|
||||
}
|
||||
analogWrite(pwm, pwm_input);
|
||||
}
|
||||
}
|
||||
|
||||
void motor_off(int motor) {
|
||||
int a;
|
||||
int b;
|
||||
int pwm;
|
||||
|
||||
if (motor == MOTORS::LIFT) {
|
||||
a = HARDWARE::MOTOR_LIFT_A;
|
||||
b = HARDWARE::MOTOR_LIFT_B;
|
||||
pwm = HARDWARE::MOTOR_LIFT_PWM;
|
||||
}
|
||||
else if (motor == MOTORS::TILT) {
|
||||
a = HARDWARE::MOTOR_TILT_A;
|
||||
b = HARDWARE::MOTOR_TILT_B;
|
||||
pwm = HARDWARE::MOTOR_TILT_PWM;
|
||||
}
|
||||
else {
|
||||
return;
|
||||
}
|
||||
|
||||
digitalWrite(a, LOW);
|
||||
digitalWrite(b, LOW);
|
||||
analogWrite(pwm, 0);
|
||||
}
|
||||
////////// Includes //////////
|
||||
#include "HX711.h"
|
||||
#include <ModbusRtu.h>
|
||||
|
||||
////////// Hardware / Data Enumerations //////////
|
||||
enum HARDWARE {
|
||||
RS485_EN = 6,
|
||||
RS485_RX = 9,
|
||||
RS485_TX = 10,
|
||||
|
||||
MOTOR_LIFT_A = 27,
|
||||
MOTOR_LIFT_B = 28,
|
||||
MOTOR_LIFT_PWM = 25,
|
||||
MOTOR_LIFT_CS = 31,
|
||||
MOTOR_LIFT_EN = 24,
|
||||
MOTOR_LIFT_FB = A10,
|
||||
|
||||
MOTOR_TILT_A = 30,
|
||||
MOTOR_TILT_B = 29,
|
||||
MOTOR_TILT_PWM = 32,
|
||||
MOTOR_TILT_CS = 26,
|
||||
MOTOR_TILT_EN = 33,
|
||||
MOTOR_TILT_FB = A11,
|
||||
|
||||
LED_13 = 13,
|
||||
|
||||
LED_RED = 20,
|
||||
LED_BLUE = 21,
|
||||
LED_GREEN = 22,
|
||||
|
||||
SCALE_DOUT = 8,
|
||||
SCALE_CLK = 7
|
||||
|
||||
};
|
||||
|
||||
enum MOTORS {
|
||||
LIFT = 0,
|
||||
TILT = 1,
|
||||
};
|
||||
|
||||
enum MODBUS_REGISTERS {
|
||||
// Inputs
|
||||
SET_POSITION_LIFT_POSITIVE = 0,
|
||||
SET_POSITION_LIFT_NEGATIVE = 1,
|
||||
SET_POSITION_TILT_POSITIVE = 2,
|
||||
SET_POSITION_TILT_NEGATIVE = 3,
|
||||
SET_POSITION_LIFT_ABSOLUTE = 4,
|
||||
SET_POSITION_TILT_ABSOLUTE = 5,
|
||||
MEASURE = 6,
|
||||
TARE = 7,
|
||||
CALIBRATION_FACTOR = 8,
|
||||
|
||||
// Outputs
|
||||
CURRENT_POSITION_LIFT = 9,
|
||||
CURRENT_POSITION_TILT = 10,
|
||||
MEASURED_WEIGHT = 11
|
||||
};
|
||||
|
||||
////////// Global Variables //////////
|
||||
int set_position_lift = 1023;
|
||||
int set_position_tilt = 350;
|
||||
int current_position_lift = 0;
|
||||
int current_position_tilt = 0;
|
||||
int tolerance = 20; //tolerance for position
|
||||
|
||||
float last_calibration_factor = -120000; //for the scale
|
||||
|
||||
// modbus stuff
|
||||
const uint8_t node_id = 2;
|
||||
const uint8_t mobus_serial_port_number = 2;
|
||||
uint16_t modbus_data[] = {0, 0, 0, 0, 9999, 9999, 0, 0, 972, 0, 0, 0};
|
||||
uint8_t num_modbus_registers = 0;
|
||||
int8_t poll_state = 0;
|
||||
bool communication_good = false;
|
||||
uint8_t message_count = 0;
|
||||
|
||||
// nice human words for motor states
|
||||
#define BRAKEVCC 0
|
||||
#define CW 1
|
||||
#define CCW 2
|
||||
#define BRAKEGND 3
|
||||
|
||||
////////// Class Instantiations //////////
|
||||
HX711 scale(HARDWARE::SCALE_DOUT, HARDWARE::SCALE_CLK);
|
||||
Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN);
|
||||
|
||||
void setup() {
|
||||
Serial.begin(9600); // debug
|
||||
// while(!Serial);
|
||||
|
||||
setup_hardware();
|
||||
num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
|
||||
slave.begin(115200); // baud-rate at 19200
|
||||
slave.setTimeOut(200);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
poll_modbus();
|
||||
set_leds();
|
||||
set_motors();
|
||||
set_scale();
|
||||
poll_scale();
|
||||
}
|
||||
|
||||
void setup_hardware() {
|
||||
pinMode(HARDWARE::RS485_EN, OUTPUT);
|
||||
|
||||
pinMode(HARDWARE::MOTOR_LIFT_A, OUTPUT);
|
||||
pinMode(HARDWARE::MOTOR_LIFT_B, OUTPUT);
|
||||
pinMode(HARDWARE::MOTOR_LIFT_PWM, OUTPUT);
|
||||
pinMode(HARDWARE::MOTOR_LIFT_EN, OUTPUT);
|
||||
pinMode(HARDWARE::MOTOR_LIFT_CS, INPUT);
|
||||
pinMode(HARDWARE::MOTOR_LIFT_FB, INPUT);
|
||||
|
||||
pinMode(HARDWARE::MOTOR_TILT_A, OUTPUT);
|
||||
pinMode(HARDWARE::MOTOR_TILT_B, OUTPUT);
|
||||
pinMode(HARDWARE::MOTOR_TILT_PWM, OUTPUT);
|
||||
pinMode(HARDWARE::MOTOR_TILT_EN, OUTPUT);
|
||||
pinMode(HARDWARE::MOTOR_TILT_CS, INPUT);
|
||||
pinMode(HARDWARE::MOTOR_TILT_FB, INPUT);
|
||||
|
||||
pinMode(HARDWARE::LED_13, OUTPUT);
|
||||
pinMode(HARDWARE::LED_RED, OUTPUT);
|
||||
pinMode(HARDWARE::LED_BLUE, OUTPUT);
|
||||
pinMode(HARDWARE::LED_GREEN, OUTPUT);
|
||||
|
||||
// set defualt states
|
||||
digitalWrite(HARDWARE::LED_RED, LOW);
|
||||
digitalWrite(HARDWARE::LED_GREEN, HIGH);
|
||||
digitalWrite(HARDWARE::LED_BLUE, HIGH);
|
||||
digitalWrite(HARDWARE::LED_13, LOW);
|
||||
digitalWrite(HARDWARE::MOTOR_LIFT_EN, HIGH);
|
||||
digitalWrite(HARDWARE::MOTOR_TILT_EN, HIGH);
|
||||
|
||||
// Change motor PWM frequency so it's not in the audible range
|
||||
analogWriteFrequency(HARDWARE::MOTOR_LIFT_PWM, 25000);
|
||||
analogWriteFrequency(HARDWARE::MOTOR_TILT_PWM, 25000);
|
||||
|
||||
// set the current desired position to the current position
|
||||
// set_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB);
|
||||
// set_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB);
|
||||
current_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB);
|
||||
current_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB);
|
||||
|
||||
// setup scale
|
||||
scale.set_scale(last_calibration_factor);
|
||||
scale.tare(); //Reset the scale to 0
|
||||
}
|
||||
|
||||
void poll_modbus(){
|
||||
poll_state = slave.poll(modbus_data, num_modbus_registers);
|
||||
communication_good = !slave.getTimeOutState();
|
||||
}
|
||||
|
||||
void set_leds(){
|
||||
if(poll_state > 4){
|
||||
message_count++;
|
||||
if(message_count > 2){
|
||||
digitalWrite(HARDWARE::LED_13, !digitalRead(HARDWARE::LED_13));
|
||||
message_count = 0;
|
||||
}
|
||||
|
||||
digitalWrite(HARDWARE::LED_GREEN, LOW);
|
||||
digitalWrite(HARDWARE::LED_RED, HIGH);
|
||||
}else if(!communication_good){
|
||||
digitalWrite(HARDWARE::LED_13, LOW);
|
||||
digitalWrite(HARDWARE::LED_GREEN, HIGH);
|
||||
digitalWrite(HARDWARE::LED_RED, LOW);
|
||||
}
|
||||
}
|
||||
|
||||
void set_motors() {
|
||||
if (modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT_ABSOLUTE] < 1024 ){
|
||||
set_position_lift = modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT_ABSOLUTE];
|
||||
modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT_ABSOLUTE] = 1024;
|
||||
}else{
|
||||
set_position_lift += modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT_POSITIVE] - modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT_NEGATIVE];
|
||||
set_position_lift = constrain(set_position_lift, 0, 1023);
|
||||
|
||||
modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT_POSITIVE] = 0;
|
||||
modbus_data[MODBUS_REGISTERS::SET_POSITION_LIFT_NEGATIVE] = 0;
|
||||
}
|
||||
|
||||
if(modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_ABSOLUTE] < 1024){
|
||||
set_position_tilt = modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_ABSOLUTE];
|
||||
modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_ABSOLUTE] = 1024;
|
||||
}else{
|
||||
set_position_tilt += modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_POSITIVE] - modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_NEGATIVE] ;
|
||||
set_position_tilt = constrain(set_position_tilt, 0, 1023);
|
||||
|
||||
modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_POSITIVE] = 0;
|
||||
modbus_data[MODBUS_REGISTERS::SET_POSITION_TILT_NEGATIVE] = 0;
|
||||
}
|
||||
|
||||
current_position_lift = analogRead(HARDWARE::MOTOR_LIFT_FB);
|
||||
current_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB);
|
||||
|
||||
modbus_data[MODBUS_REGISTERS::CURRENT_POSITION_LIFT] = current_position_lift;
|
||||
modbus_data[MODBUS_REGISTERS::CURRENT_POSITION_TILT] = current_position_tilt;
|
||||
|
||||
if (abs(current_position_lift - set_position_lift) > tolerance) {
|
||||
if (current_position_lift < set_position_lift) {
|
||||
set_motor_output(MOTORS::LIFT, CCW, 255);
|
||||
}
|
||||
else {
|
||||
set_motor_output(MOTORS::LIFT, CW, 255);
|
||||
}
|
||||
}
|
||||
else {
|
||||
motor_off(MOTORS::LIFT);
|
||||
}
|
||||
|
||||
if (abs(current_position_tilt - set_position_tilt) > tolerance) {
|
||||
if (current_position_tilt < set_position_tilt) {
|
||||
set_motor_output(MOTORS::TILT, CCW, 255);
|
||||
}
|
||||
else {
|
||||
set_motor_output(MOTORS::TILT, CW, 255);
|
||||
}
|
||||
}
|
||||
else {
|
||||
motor_off(MOTORS::TILT);
|
||||
}
|
||||
}
|
||||
|
||||
void set_scale(){
|
||||
float cal_factor = modbus_data[MODBUS_REGISTERS::CALIBRATION_FACTOR] * -100;
|
||||
if(cal_factor != last_calibration_factor){
|
||||
scale.set_scale(cal_factor);
|
||||
last_calibration_factor = cal_factor;
|
||||
}
|
||||
|
||||
if (modbus_data[MODBUS_REGISTERS::TARE] == 1){
|
||||
scale.tare();
|
||||
modbus_data[MODBUS_REGISTERS::TARE] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void poll_scale(){
|
||||
if(modbus_data[MODBUS_REGISTERS::MEASURE] == 1){
|
||||
// Serial.println(scale.get_units()*-1000);
|
||||
modbus_data[MODBUS_REGISTERS::MEASURED_WEIGHT] = scale.get_units()*-1000;
|
||||
modbus_data[MODBUS_REGISTERS::MEASURE] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
//---Set Motor Output---//
|
||||
/*
|
||||
Inputs: motor number, direction, pwm value
|
||||
Returns: nothing
|
||||
|
||||
Will set a motor going in a specific direction the motor will continue
|
||||
going in that direction, at that speed until told to do otherwise.
|
||||
|
||||
direct: Should be between 0 and 3, with the following result
|
||||
0: Brake to VCC
|
||||
1: Clockwise
|
||||
2: CounterClockwise
|
||||
3: Brake to GND
|
||||
|
||||
pwm: should be a value between 0 and 255, higher the number, the faster
|
||||
it'll go
|
||||
----------------
|
||||
Control Logic:
|
||||
----------------
|
||||
A | B
|
||||
Brake VCC: 1 1
|
||||
CW: 1 0
|
||||
CCW: 0 1
|
||||
Brake GND: 0 0
|
||||
----------------
|
||||
*/
|
||||
void set_motor_output(int motor, int direction, int pwm_input) {
|
||||
|
||||
int a;
|
||||
int b;
|
||||
int pwm;
|
||||
|
||||
if (motor == MOTORS::LIFT) {
|
||||
a = HARDWARE::MOTOR_LIFT_A;
|
||||
b = HARDWARE::MOTOR_LIFT_B;
|
||||
pwm = HARDWARE::MOTOR_LIFT_PWM;
|
||||
}
|
||||
else if (motor == MOTORS::TILT) {
|
||||
a = HARDWARE::MOTOR_TILT_A;
|
||||
b = HARDWARE::MOTOR_TILT_B;
|
||||
pwm = HARDWARE::MOTOR_TILT_PWM;
|
||||
}
|
||||
else {
|
||||
return;
|
||||
}
|
||||
|
||||
if (direction <= 4) {
|
||||
// Set A
|
||||
if (direction <= 1) {
|
||||
digitalWrite(a, HIGH);
|
||||
}
|
||||
else {
|
||||
digitalWrite(a, LOW);
|
||||
}
|
||||
|
||||
// Set B
|
||||
if ((direction == 0) || (direction == 2)) {
|
||||
digitalWrite(b, HIGH);
|
||||
}
|
||||
else {
|
||||
digitalWrite(b, LOW);
|
||||
}
|
||||
analogWrite(pwm, pwm_input);
|
||||
}
|
||||
}
|
||||
|
||||
void motor_off(int motor) {
|
||||
int a;
|
||||
int b;
|
||||
int pwm;
|
||||
|
||||
if (motor == MOTORS::LIFT) {
|
||||
a = HARDWARE::MOTOR_LIFT_A;
|
||||
b = HARDWARE::MOTOR_LIFT_B;
|
||||
pwm = HARDWARE::MOTOR_LIFT_PWM;
|
||||
}
|
||||
else if (motor == MOTORS::TILT) {
|
||||
a = HARDWARE::MOTOR_TILT_A;
|
||||
b = HARDWARE::MOTOR_TILT_B;
|
||||
pwm = HARDWARE::MOTOR_TILT_PWM;
|
||||
}
|
||||
else {
|
||||
return;
|
||||
}
|
||||
|
||||
digitalWrite(a, LOW);
|
||||
digitalWrite(b, LOW);
|
||||
analogWrite(pwm, 0);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user