caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 10:11:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

Lots o changes. Too tired to think

Browse Source
This commit is contained in:
Corwin Perren
2018-08-05 11:23:26 -07:00
parent 91be05baff
commit e688aff233
15 changed files with 933 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

165
software/firmware/rdf/rdf.ino
View File

@@ -3,151 +3,86 @@
////////// Hardware / Data Enumerations //////////
enum HARDWARE {
RS485_EN = 2,
RS485_RX = 7,
RS485_TX = 8,
COMMS_RS485_EN = 3,
COMMS_RS485_RX = 9,
COMMS_RS485_TX = 10,
MOTOR_CURRENT_SENSE = A4,
MOTOR_DIRECTION = 19,
MOTOR_PWM = 20,
MOTOR_SLEEP = 21,
MOTOR_FAULT = 22,
// COMMS_RS485_EN = 2,
// COMMS_RS485_RX = 0,
// COMMS_RS485_TX = 1,
TEMP = A9,
RDF_INPUT = A7,
LED_RED = 1,
LED_GREEN = 32,
LED_BLUE = 6,
LED_BLUE_EXTRA = 13
LED_BLUE_EXTRA = 13
};
enum MODBUS_REGISTERS {
DIRECTION = 0, // Input
SPEED = 1, // Input
SLEEP = 2, // Input
CURRENT = 3, // Output
FAULT = 4, // Output
TEMPERATURE = 5 // Output
RAW_DATA = 0
};
////////// Global Variables //////////
const uint8_t node_id = 2;
const uint8_t mobus_serial_port_number = 3;
uint16_t modbus_data[] = {0, 0, 0, 0, 0, 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;
uint16_t rampdown_step = 2000;
////////// Class Instantiations //////////
Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN);
Modbus slave(node_id, modbus_serial_port_number, HARDWARE::COMMS_RS485_EN);
void setup() {
setup_hardware();
// Debugging
// Serial.begin(9600);
// while (!Serial);
num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
slave.begin(115200); // baud-rate at 19200
slave.setTimeOut(150);
// 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);
Serial.begin(9600);
}
void loop() {
poll_modbus();
set_leds();
set_motor();
poll_sensors_and_motor_state();
// Do normal polling
poll_modbus();
set_leds();
}
void setup_hardware(){
// Setup pins as inputs / outputs
pinMode(HARDWARE::RS485_EN, OUTPUT);
pinMode(HARDWARE::MOTOR_CURRENT_SENSE, INPUT);
pinMode(HARDWARE::MOTOR_DIRECTION, OUTPUT);
pinMode(HARDWARE::MOTOR_PWM, OUTPUT);
pinMode(HARDWARE::MOTOR_SLEEP, OUTPUT);
pinMode(HARDWARE::MOTOR_FAULT, INPUT);
void setup_hardware() {
// Setup pins as inputs / outputs
pinMode(HARDWARE::RDF_INPUT, INPUT);
pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT);
pinMode(HARDWARE::TEMP, INPUT);
analogReadResolution(13);
pinMode(HARDWARE::LED_RED, OUTPUT);
pinMode(HARDWARE::LED_GREEN, OUTPUT);
pinMode(HARDWARE::LED_BLUE, OUTPUT);
}
pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT);
void poll_modbus() {
poll_state = slave.poll(modbus_data, num_modbus_registers);
communication_good = !slave.getTimeOutState();
// Set default pin states
digitalWrite(HARDWARE::MOTOR_SLEEP, HIGH);
digitalWrite(HARDWARE::LED_RED, LOW);
digitalWrite(HARDWARE::LED_GREEN, HIGH);
digitalWrite(HARDWARE::LED_BLUE, HIGH);
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);
// Set the PWM resolution to 16-bits
analogWriteResolution(16);
// Change motor PWM frequency so it's not in the audible range
analogWriteFrequency(HARDWARE::MOTOR_PWM, 25000);
// Set teensy to increased analog resolution
analogReadResolution(13);
}
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;
}
digitalWrite(HARDWARE::LED_GREEN, LOW);
digitalWrite(HARDWARE::LED_RED, HIGH);
}else if(!communication_good){
digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
digitalWrite(HARDWARE::LED_GREEN, HIGH);
digitalWrite(HARDWARE::LED_RED, LOW);
}
}
void set_motor(){
if(communication_good){
digitalWrite(HARDWARE::MOTOR_DIRECTION, modbus_data[MODBUS_REGISTERS::DIRECTION]);
analogWrite(HARDWARE::MOTOR_PWM, modbus_data[MODBUS_REGISTERS::SPEED]);
digitalWrite(HARDWARE::MOTOR_SLEEP, modbus_data[MODBUS_REGISTERS::SLEEP]);
}else{
while(modbus_data[MODBUS_REGISTERS::SPEED] != 0 && modbus_data[MODBUS_REGISTERS::SPEED] > rampdown_step){
modbus_data[MODBUS_REGISTERS::SPEED] -= rampdown_step;
analogWrite(HARDWARE::MOTOR_PWM, modbus_data[MODBUS_REGISTERS::SPEED]);
delay(2);
}
modbus_data[MODBUS_REGISTERS::SPEED] = 0;
analogWrite(HARDWARE::MOTOR_PWM, modbus_data[MODBUS_REGISTERS::SPEED]);
}
}
void poll_sensors_and_motor_state(){
// Not the most elegant calculations, could clean up.
modbus_data[MODBUS_REGISTERS::CURRENT] = (uint16_t)(((((analogRead(HARDWARE::MOTOR_CURRENT_SENSE) / 8192.0) * 3.3) - 0.05) / 0.02) * 1000);
modbus_data[MODBUS_REGISTERS::FAULT] = !digitalRead(HARDWARE::MOTOR_FAULT);
modbus_data[MODBUS_REGISTERS::TEMPERATURE] = (uint16_t)(((((analogRead(HARDWARE::TEMP) / 8192.0) * 3.3) - 0.750) / 0.01) * 1000);
Serial.println(modbus_data[MODBUS_REGISTERS::CURRENT]);
}
}

172
software/firmware/temp/new_rdf.ino Normal file
View 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);
}
}