mirror of
https://github.com/OSURoboticsClub/Rover_2017_2018.git
synced 2025-11-08 10:11:14 +00:00
Added IONI configs and mining firmware
This commit is contained in:
Dylan Thrush
@@ -21,7 +21,7 @@ enum HARDWARE {
|
||||
};
|
||||
|
||||
////////// Global Variables //////////
|
||||
// define user words for motor states
|
||||
// define nice words for motor states
|
||||
#define BRAKE 0
|
||||
#define FWD 1
|
||||
#define REV 2
|
||||
@@ -48,11 +48,12 @@ double signed_setpoint_vel = -200;
|
||||
// Position
|
||||
double setpoint_pos = 0; //PID position variables
|
||||
double input_pos, output_pos;
|
||||
PID motor_pos(&input_pos, &output_pos, &setpoint_pos, 10, 0, 0, DIRECT); //position PID
|
||||
long current_pos = 1000;
|
||||
PID motor_pos(&input_pos, &output_pos, &setpoint_pos, 500, 0, 0, DIRECT); //position PID
|
||||
|
||||
long current_pos = 0;
|
||||
|
||||
// Homing
|
||||
int trip_threshold = 650;
|
||||
int trip_threshold = 550;
|
||||
bool home_flag = true;
|
||||
long prev_millis = 0;
|
||||
|
||||
@@ -72,68 +73,81 @@ void setup() {
|
||||
void loop() {
|
||||
|
||||
// Position based homing
|
||||
//Serial.println(analogRead(HARDWARE::MOTOR_CURRENT_SENSE));
|
||||
// if (home_flag == true){
|
||||
// Serial.println("homing");
|
||||
// Serial.println(analogRead(HARDWARE::MOTOR_CURRENT_SENSE));
|
||||
//
|
||||
// unsigned long curr_millis = millis();
|
||||
// // if it is time to move the motor and we haven't tripped the current yet:
|
||||
// unsigned long curr_millis = millis();
|
||||
// if (curr_millis - prev_millis > interval){
|
||||
// prev_millis = curr_millis;
|
||||
// current_pos += 5; // increment by counts
|
||||
//
|
||||
// if (analogRead(HARDWARE::MOTOR_CURRENT_SENSE) < trip_threshold){
|
||||
// current_pos = 0;
|
||||
// enc.write(0);
|
||||
// home_flag = false;
|
||||
// current_pos -= 10; // increment by counts
|
||||
// }
|
||||
// if ((analogRead(HARDWARE::MOTOR_CURRENT_SENSE) > trip_threshold) && (curr_millis > 1000)){
|
||||
// enc.write(0);
|
||||
// current_pos = 0;
|
||||
// home_flag = false;
|
||||
// Serial.println("homing complete");
|
||||
// }
|
||||
// }
|
||||
|
||||
// Position movement test
|
||||
// unsigned long curr_millis = millis();
|
||||
// // if it is time to move the motor and we haven't tripped the current yet:
|
||||
// if (curr_millis - prev_millis > interval){
|
||||
// prev_millis = curr_millis;
|
||||
// current_pos += 1; // increment by counts
|
||||
// }
|
||||
//
|
||||
// set_position(10000);
|
||||
//
|
||||
// set_position(current_pos);
|
||||
// Serial.print("Set Pos: ");
|
||||
// Serial.println(current_pos);
|
||||
// Serial.print("Curr Pos: ");
|
||||
// Serial.println(enc.read());
|
||||
// delay(1);
|
||||
|
||||
// Velocity based non blocking homing
|
||||
if (home_flag == true){
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
set_velocity(-150);
|
||||
// Position movement test
|
||||
// unsigned long curr_millis = millis();
|
||||
// // if it is time to move the motor and we haven't tripped the current yet:
|
||||
// if (curr_millis - prev_millis > interval){
|
||||
// prev_millis = curr_millis;
|
||||
|
||||
Serial.println("Homing");
|
||||
// current_pos += 10; // increment by counts
|
||||
|
||||
Serial.print("Current: ");
|
||||
Serial.println(analogRead(HARDWARE::MOTOR_CURRENT_SENSE));
|
||||
|
||||
if ((analogRead(HARDWARE::MOTOR_CURRENT_SENSE) > trip_threshold) && (millis() > 500)){
|
||||
Serial.println("Tripped current sensor!");
|
||||
set_motor_output(BRAKE, 0); //stop moving
|
||||
home_flag = false; // no more homing
|
||||
//set_motor_output(COAST, 0); //stop moving
|
||||
enc.write(0); // reset encoder
|
||||
}
|
||||
}
|
||||
else {
|
||||
set_position(current_pos); // hold
|
||||
Serial.print("Set Pos: ");
|
||||
Serial.println(current_pos);
|
||||
Serial.print("Curr Pos: ");
|
||||
Serial.println(enc.read());
|
||||
}
|
||||
// }
|
||||
//
|
||||
set_position(current_pos);
|
||||
|
||||
Serial.print("Set Pos: ");
|
||||
Serial.println(current_pos);
|
||||
Serial.print("Curr Pos: ");
|
||||
Serial.println(enc.read());
|
||||
delay(1);
|
||||
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
// Velocity based non blocking homing
|
||||
// if (home_flag == true){
|
||||
//
|
||||
// set_velocity(-150);
|
||||
//
|
||||
// Serial.println("Homing");
|
||||
//
|
||||
// Serial.print("Current: ");
|
||||
// Serial.println(analogRead(HARDWARE::MOTOR_CURRENT_SENSE));
|
||||
//
|
||||
// if ((analogRead(HARDWARE::MOTOR_CURRENT_SENSE) > trip_threshold) && (millis() > 500)){
|
||||
// Serial.println("Tripped current sensor!");
|
||||
// set_motor_output(BRAKE, 0); //stop moving
|
||||
// home_flag = false; // no more homing
|
||||
// //set_motor_output(COAST, 0); //stop moving
|
||||
// enc.write(0); // reset encoder
|
||||
// }
|
||||
// }
|
||||
// else {
|
||||
// set_position(current_pos); // hold
|
||||
// Serial.print("Set Pos: ");
|
||||
// Serial.println(current_pos);
|
||||
// Serial.print("Curr Pos: ");
|
||||
// Serial.println(enc.read());
|
||||
// }
|
||||
//
|
||||
// delay(1);
|
||||
|
||||
}
|
||||
|
||||
void setup_hardware(){
|
||||
|
||||
289
software/firmware/mining/mining.ino
Normal file
289
software/firmware/mining/mining.ino
Normal file
@@ -0,0 +1,289 @@
|
||||
////////// 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);
|
||||
}
|
||||
Reference in New Issue
Block a user