mirror of
https://github.com/OSURoboticsClub/Rover_2017_2018.git
synced 2025-11-08 10:11:14 +00:00
Merge branch 'master' of https://github.com/OSURoboticsClub/Rover_2017_2018
This commit is contained in:
Dylan Thrush
@@ -1,302 +1,335 @@
|
||||
////////// 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 = 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 current_position_lift = 0;
|
||||
int current_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 = 2;
|
||||
uint16_t modbus_data[] = {1023, 350, 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 //////////
|
||||
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
|
||||
|
||||
setup_hardware();
|
||||
num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
|
||||
slave.begin(115200); // baud-rate at 19200
|
||||
slave.setTimeOut(500);
|
||||
}
|
||||
|
||||
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(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() {
|
||||
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);
|
||||
|
||||
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(){
|
||||
scale.set_scale(modbus_data[MODBUS_REGISTERS::CALIBRATION_FACTOR]*-1000);
|
||||
|
||||
if (modbus_data[MODBUS_REGISTERS::TARE] == 1){
|
||||
scale.tare();
|
||||
modbus_data[MODBUS_REGISTERS::TARE] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void poll_scale(){
|
||||
modbus_data[MODBUS_REGISTERS::MEASURED_WEIGHT] = scale.get_units()*-1000;
|
||||
}
|
||||
|
||||
//---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);
|
||||
}
|
||||
|
||||
88
software/firmware/rdf/rdf.ino
Normal file
88
software/firmware/rdf/rdf.ino
Normal file
@@ -0,0 +1,88 @@
|
||||
////////// Includes //////////
|
||||
#include <ModbusRtu.h>
|
||||
|
||||
////////// Hardware / Data Enumerations //////////
|
||||
enum HARDWARE {
|
||||
COMMS_RS485_EN = 3,
|
||||
COMMS_RS485_RX = 9,
|
||||
COMMS_RS485_TX = 10,
|
||||
|
||||
// COMMS_RS485_EN = 2,
|
||||
// COMMS_RS485_RX = 0,
|
||||
// COMMS_RS485_TX = 1,
|
||||
|
||||
RDF_INPUT = A7,
|
||||
|
||||
LED_BLUE_EXTRA = 13
|
||||
};
|
||||
|
||||
enum MODBUS_REGISTERS {
|
||||
RAW_DATA = 0
|
||||
};
|
||||
|
||||
|
||||
////////// Global Variables //////////
|
||||
///// Modbus
|
||||
const uint8_t node_id = 1;
|
||||
const uint8_t modbus_serial_port_number = 2;
|
||||
|
||||
uint16_t modbus_data[] = {0};
|
||||
uint8_t num_modbus_registers = 0;
|
||||
|
||||
int8_t poll_state = 0;
|
||||
bool communication_good = false;
|
||||
uint8_t message_count = 0;
|
||||
|
||||
////////// Class Instantiations //////////
|
||||
Modbus slave(node_id, modbus_serial_port_number, HARDWARE::COMMS_RS485_EN);
|
||||
|
||||
void setup() {
|
||||
// Debugging
|
||||
// Serial.begin(9600);
|
||||
// while (!Serial);
|
||||
|
||||
// Raw pin setup
|
||||
setup_hardware();
|
||||
|
||||
// Setup modbus serial communication
|
||||
num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
|
||||
slave.begin(115200); // baud-rate at 19200
|
||||
slave.setTimeOut(100);
|
||||
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// Do normal polling
|
||||
poll_modbus();
|
||||
set_leds();
|
||||
|
||||
}
|
||||
|
||||
|
||||
void setup_hardware() {
|
||||
// Setup pins as inputs / outputs
|
||||
pinMode(HARDWARE::RDF_INPUT, INPUT);
|
||||
pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT);
|
||||
|
||||
analogReadResolution(13);
|
||||
|
||||
}
|
||||
|
||||
void poll_modbus() {
|
||||
poll_state = slave.poll(modbus_data, num_modbus_registers);
|
||||
communication_good = !slave.getTimeOutState();
|
||||
|
||||
modbus_data[MODBUS_REGISTERS::RAW_DATA] = analogRead(HARDWARE::RDF_INPUT);
|
||||
}
|
||||
|
||||
void set_leds() {
|
||||
if (poll_state > 4) {
|
||||
message_count++;
|
||||
if (message_count > 2) {
|
||||
digitalWrite(HARDWARE::LED_BLUE_EXTRA, !digitalRead(HARDWARE::LED_BLUE_EXTRA));
|
||||
message_count = 0;
|
||||
}
|
||||
} else if (!communication_good) {
|
||||
digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
|
||||
}
|
||||
}
|
||||
172
software/firmware/temp/new_rdf.ino
Normal file
172
software/firmware/temp/new_rdf.ino
Normal file
@@ -0,0 +1,172 @@
|
||||
////////// Includes //////////
|
||||
#include <ModbusRtu.h>
|
||||
|
||||
////////// Hardware / Data Enumerations //////////
|
||||
enum HARDWARE {
|
||||
COMMS_RS485_EN = 3,
|
||||
COMMS_RS485_RX = 9,
|
||||
COMMS_RS485_TX = 10,
|
||||
|
||||
// COMMS_RS485_EN = 2,
|
||||
// COMMS_RS485_RX = 0,
|
||||
// COMMS_RS485_TX = 1,
|
||||
|
||||
RDF_INPUT = A7,
|
||||
|
||||
LED_BLUE_EXTRA = 13
|
||||
};
|
||||
|
||||
enum MODBUS_REGISTERS {
|
||||
SENSITIVITY = 0,
|
||||
RAW_DATA = 1, // Input
|
||||
CLEAN_DATA_POSITIVE = 2,
|
||||
CLEAN_DATA_NEGATIVE = 3,
|
||||
FREQUENCY = 4,
|
||||
};
|
||||
|
||||
|
||||
////////// Global Variables //////////
|
||||
///// Modbus
|
||||
const uint8_t node_id = 1;
|
||||
const uint8_t modbus_serial_port_number = 2;
|
||||
|
||||
uint16_t modbus_data[] = {50, 0, 0, 0, 0};
|
||||
uint8_t num_modbus_registers = 0;
|
||||
|
||||
int8_t poll_state = 0;
|
||||
bool communication_good = false;
|
||||
uint8_t message_count = 0;
|
||||
|
||||
|
||||
//////////////// Anothony's stuff /////////////
|
||||
int state;
|
||||
float freq;
|
||||
float ambientNoise;
|
||||
unsigned long totalDataPoints;
|
||||
int dataBuff[3];
|
||||
int data[3];
|
||||
unsigned long t1,t2,t3;
|
||||
int dt1,dt2 =0;
|
||||
float dtavg;
|
||||
int tcnt =2;
|
||||
bool upstate = false;
|
||||
|
||||
|
||||
////////// Class Instantiations //////////
|
||||
Modbus slave(node_id, modbus_serial_port_number, HARDWARE::COMMS_RS485_EN);
|
||||
|
||||
void setup() {
|
||||
// Debugging
|
||||
Serial.begin(9600);
|
||||
while (!Serial);
|
||||
|
||||
// Raw pin setup
|
||||
setup_hardware();
|
||||
|
||||
// Setup modbus serial communication
|
||||
num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
|
||||
slave.begin(115200); // baud-rate at 19200
|
||||
slave.setTimeOut(150);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
anthonys_rdf_code();
|
||||
// Do normal polling
|
||||
poll_modbus();
|
||||
set_leds();
|
||||
|
||||
}
|
||||
|
||||
void anthonys_rdf_code(){
|
||||
modbus_data[MODBUS_REGISTERS::RAW_DATA] = analogRead(HARDWARE::RDF_INPUT);
|
||||
|
||||
for(int i=0;i<3;i++){
|
||||
dataBuff[i] = data[i];
|
||||
}
|
||||
dataSet();
|
||||
int change = changeCheck();
|
||||
if(change){
|
||||
state = change;
|
||||
if(change == 1){
|
||||
t1 = millis();
|
||||
dt1 = t1-t2;
|
||||
}else{
|
||||
t2 = millis();
|
||||
dt2 = t2-t1;
|
||||
}
|
||||
if(dt2>dt1-100&&dt1>dt2-100){
|
||||
dtavg = (dtavg*float(tcnt-2)/float(tcnt))+(((dt1+dt2)/2.0)*float(2.0/tcnt));
|
||||
tcnt += 2;
|
||||
freq = 500.0/dtavg;
|
||||
}
|
||||
}
|
||||
if(change ==1 || millis() > dtavg*2+t3){
|
||||
t3 = millis();
|
||||
upstate = true;
|
||||
}
|
||||
if(millis()<t3+dtavg){
|
||||
for(int i=0;i<3;i++){
|
||||
int out_data = data[i]-ambientNoise;
|
||||
|
||||
if(out_data >=0){
|
||||
modbus_data[MODBUS_REGISTERS::CLEAN_DATA_NEGATIVE] = 0;
|
||||
modbus_data[MODBUS_REGISTERS::CLEAN_DATA_POSITIVE] = out_data;
|
||||
}else{
|
||||
modbus_data[MODBUS_REGISTERS::CLEAN_DATA_POSITIVE] = 0;
|
||||
modbus_data[MODBUS_REGISTERS::CLEAN_DATA_NEGATIVE] = out_data;
|
||||
}
|
||||
modbus_data[MODBUS_REGISTERS::FREQUENCY] = freq * 100;
|
||||
// Serial.print(data[i]-ambientNoise);
|
||||
// Serial.print(", ");
|
||||
Serial.println(freq);
|
||||
}
|
||||
}else{
|
||||
float avgdat,dtot =0;
|
||||
for(int i=0;i<3;i++)
|
||||
dtot+=data[i];
|
||||
avgdat = dtot/3.0;
|
||||
ambientNoise = ambientNoise*float(totalDataPoints)/float(totalDataPoints+3)+avgdat*(3.0/float(totalDataPoints+3));
|
||||
}
|
||||
}
|
||||
|
||||
void dataSet(){
|
||||
for(int i=0;i<3;i++){
|
||||
data[i]=analogRead(HARDWARE::RDF_INPUT);
|
||||
delay(1);
|
||||
}
|
||||
}
|
||||
|
||||
int changeCheck(){
|
||||
uint16_t sensitivity = modbus_data[MODBUS_REGISTERS::SENSITIVITY];
|
||||
int newSignalState = 0; // 0= no change 1= signal start 2= signal stop
|
||||
if((data[0])>(dataBuff[2]+sensitivity)||data[2] > data[0]+sensitivity)
|
||||
newSignalState = 1;
|
||||
if((data[2] < data[0]-sensitivity)||(data[0]<(dataBuff[2]-sensitivity)))
|
||||
newSignalState = 2;
|
||||
return newSignalState;
|
||||
|
||||
}
|
||||
|
||||
void setup_hardware() {
|
||||
// Setup pins as inputs / outputs
|
||||
pinMode(HARDWARE::RDF_INPUT, INPUT);
|
||||
pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT);
|
||||
|
||||
}
|
||||
|
||||
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_BLUE_EXTRA, !digitalRead(HARDWARE::LED_BLUE_EXTRA));
|
||||
message_count = 0;
|
||||
}
|
||||
} else if (!communication_good) {
|
||||
digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
|
||||
}
|
||||
}
|
||||
84
software/firmware/temp/original_rdf.ino
Normal file
84
software/firmware/temp/original_rdf.ino
Normal file
@@ -0,0 +1,84 @@
|
||||
#define Rpin A5 //Reciever pin is A1
|
||||
int sensitivity = 50; //the lower this number, the more sensitive.
|
||||
int state; //=1 if can hear ping =2 if cannot hear ping
|
||||
float freq;
|
||||
float ambientNoise;
|
||||
unsigned long totalDataPoints;
|
||||
int dataBuff[3];
|
||||
int data[3];
|
||||
unsigned long t1,t2,t3;
|
||||
int dt1,dt2 =0;
|
||||
float dtavg;
|
||||
int tcnt =2;
|
||||
bool upstate = false;
|
||||
bool mode = false; //switches between two modes (t/f)
|
||||
void setup() {
|
||||
pinMode(Rpin, INPUT);
|
||||
Serial.begin(9600);
|
||||
totalDataPoints = 0;
|
||||
state = 2;
|
||||
}
|
||||
void dataSet(){
|
||||
for(int i=0;i<3;i++){
|
||||
data[i]=analogRead(Rpin);
|
||||
delay(1);
|
||||
}
|
||||
}
|
||||
int changeCheck(){
|
||||
int newSignalState = 0; // 0= no change 1= signal start 2= signal stop
|
||||
if((data[0])>(dataBuff[2]+sensitivity)||data[2] > data[0]+sensitivity)
|
||||
newSignalState = 1;
|
||||
if((data[2] < data[0]-sensitivity)||(data[0]<(dataBuff[2]-sensitivity)))
|
||||
newSignalState = 2;
|
||||
return newSignalState;
|
||||
|
||||
}
|
||||
void loop() {
|
||||
|
||||
if(Serial.available()){
|
||||
Serial.read();
|
||||
mode = !mode;
|
||||
}
|
||||
if(mode == true){ //mode 1 is raw output from the antena.
|
||||
Serial.println(analogRead(Rpin));
|
||||
delay(1);
|
||||
}
|
||||
else{
|
||||
for(int i=0;i<3;i++)
|
||||
dataBuff[i] = data[i];
|
||||
dataSet();
|
||||
int change = changeCheck();
|
||||
if(change){
|
||||
state = change;
|
||||
if(change == 1){
|
||||
t1 = millis();
|
||||
dt1 = t1-t2;
|
||||
}else{
|
||||
t2 = millis();
|
||||
dt2 = t2-t1;
|
||||
}
|
||||
if(dt2>dt1-100&&dt1>dt2-100){
|
||||
dtavg = (dtavg*float(tcnt-2)/float(tcnt))+(((dt1+dt2)/2.0)*float(2.0/tcnt));
|
||||
tcnt += 2;
|
||||
freq = 500.0/dtavg;
|
||||
}
|
||||
}
|
||||
if(change ==1 || millis() > dtavg*2+t3){
|
||||
t3 = millis();
|
||||
upstate = true;
|
||||
}
|
||||
if(millis()<t3+dtavg){
|
||||
for(int i=0;i<3;i++){
|
||||
Serial.print(data[i]-ambientNoise);
|
||||
Serial.print(", ");
|
||||
Serial.println(freq);
|
||||
}
|
||||
}else{
|
||||
float avgdat,dtot =0;
|
||||
for(int i=0;i<3;i++)
|
||||
dtot+=data[i];
|
||||
avgdat = dtot/3.0;
|
||||
ambientNoise = ambientNoise*float(totalDataPoints)/float(totalDataPoints+3)+avgdat*(3.0/float(totalDataPoints+3));
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user