From 8f1cd9f86e60504d1c92e09516fb8e0127d9c6d4 Mon Sep 17 00:00:00 2001 From: Dakota Miller Date: Fri, 28 Jan 2022 18:14:47 -0600 Subject: [PATCH 01/14] Added names for the pins and assigned them to the correct TIVA pins --- powerboardcode/powerboard/powerboard.h | 80 ++++++++++++++------------ 1 file changed, 43 insertions(+), 37 deletions(-) diff --git a/powerboardcode/powerboard/powerboard.h b/powerboardcode/powerboard/powerboard.h index 5e15ccd..cafd733 100644 --- a/powerboardcode/powerboard/powerboard.h +++ b/powerboardcode/powerboard/powerboard.h @@ -2,49 +2,56 @@ #include "Energia.h" #include "RoveComm.h" //12V Current Sensing -#define AUX_MOUNT_SENSE PK_3 -#define MULTIMEDIA_SENSE PK_2 -#define LOW_CURRENT_SENSE PK_1 -#define GIMBAL_SENSE PK_0 +#define MULTIMEDIA_SENSE PD4 +#define NAV_SENSE PD0 +#define GIMBAL_ACT_SENSE PD1 +#define DRIVE_SENSE PK3 +#define SCISENSOR_ACT_SENSE PD2 +#define NETSWITCH_SENSE PB5 +#define CAM1_SENSE PB4 +#define CAM2_SENSE PK0 +#define BBB_SENSE PD7 +#define AUX_LOG_SENSE PK2 //12V CTL -#define MULTIMEDIA_LOG_CTL PK_5 -#define MULTIMEDIA_ACT_CTL PM_0 -#define AUX_LOG_CTL PM_1 -#define AUX_ACT_CTL PM_2 -#define GIMBAL_LOG_CTL PH_0 -#define GIMBAL_ACT_CTL PH_1 -#define NAV_BOARD_CTL PK_7 -#define CAM_CTL PP_3 -#define EXTRA_CTL PQ_1 -#define DRIVE_CTL PM_6 +#define MULTIMEDIA_CTL PL1 +#define NAV_CTL PN4 +#define GIMBAL_ACT_CTL PA4 +#define GIMBAL_LOG_CTL PG0 +#define DRIVE_CTL PL2 +#define SCISENSOR_ACT_CTL PL3 +#define SCISENSOR_LOG_CTL PF3 +#define NETSWITCH_CTL PL0 +#define CAM1_CTL PF1 +#define CAM2_CTL PF2 +#define BBB_CTL PN5 +#define AUX_LOG_CTL PB2 +#define SPARE_CTL PL5 //PACK CURRENT -#define P_MOTOR1_SENSE PD_5 -#define P_MOTOR2_SENSE PD_4 -#define P_MOTOR3_SENSE PD_2 -#define P_MOTOR4_SENSE PD_3 -#define P_SPARE_SENSE PE_4 -#define P_ROCKET_SENSE PD_0 -#define P_TWELVE_SENSE PD_1 -#define P_VACUUM_SENSE PE_1 -#define P_DRIVE_SENSE PE_5 -#define P_AUX_SENSE PE_3 +#define P_MOTOR1_SENSE PE4 +#define P_MOTOR2_SENSE PE5 +#define P_MOTOR3_SENSE PD3 +#define P_MOTOR4_SENSE PE0 +#define P_MOTOR5_SENSE PE1 +#define P_MOTOR6_SENSE PE2 +#define P_MOTOR7_SENSE PE3 +#define P_POE_SENSE PD5 +#define P_AUX_SENSE PK1 //PACK BUSSES -#define P_MOTOR1_CTL PQ_0 -#define P_MOTOR2_CTL PP_1 -#define P_MOTOR3_CTL PP_0 -#define P_MOTOR4_CTL PC_7 -#define P_SPARE_CTL PC_4 -#define P_ROCKET_CTL PN_2 -#define P_TWELVE_CTL PH_3 -#define P_VACUUM_CTL PE_0 -#define P_DRIVE_CTL PC_6 -#define P_AUX_CTL PE_2 +#define P_MOTOR1_CTL PC4 +#define P_MOTOR2_CTL PC5 +#define P_MOTOR3_CTL PC6 +#define P_MOTOR4_CTL PC7 +#define P_MOTOR5_CTL PP0 +#define P_MOTOR6_CTL PP1 +#define P_MOTOR7_CTL PQ0 +#define P_POE_CTL PP4 +#define P_AUX_CTL PL4 -#define MOTOR_DELAY 5000 -#define DRIVE_DELAY 5000 +#define MOTOR_DELAY 5000 +#define DRIVE_DELAY 5000 uint8_t currentSense12V[4] = { AUX_MOUNT_SENSE, MULTIMEDIA_SENSE, GIMBAL_SENSE, LOW_CURRENT_SENSE}; uint8_t actuation12V[3] = { GIMBAL_ACT_CTL, MULTIMEDIA_ACT_CTL, AUX_ACT_CTL}; @@ -59,4 +66,3 @@ uint8_t bussesMotor[5] = {P_MOTOR1_CTL, P_MOTOR2_CTL, P_MOTOR3_CTL, P_MOTOR4_CTL RoveCommEthernet RoveComm; rovecomm_packet packet; EthernetServer TCPServer(RC_ROVECOMM_POWERBOARD_PORT); - From 8449330da22befb91ea2a664dc7bdcbeac6a5567 Mon Sep 17 00:00:00 2001 From: Dakota Miller Date: Fri, 28 Jan 2022 18:43:17 -0600 Subject: [PATCH 02/14] Updated read_CurrentSense function --- powerboardcode/powerboard/powerboard.ino | 59 +++++++++++++++++------- 1 file changed, 42 insertions(+), 17 deletions(-) diff --git a/powerboardcode/powerboard/powerboard.ino b/powerboardcode/powerboard/powerboard.ino index b7721ef..1a94649 100644 --- a/powerboardcode/powerboard/powerboard.ino +++ b/powerboardcode/powerboard/powerboard.ino @@ -176,13 +176,22 @@ void read_CurrentSense() int motor2_sense = analogRead(P_MOTOR2_SENSE); int motor3_sense = analogRead(P_MOTOR3_SENSE); int motor4_sense = analogRead(P_MOTOR4_SENSE); - int spare_sense = analogRead(P_SPARE_SENSE); - int rocket_sense = analogRead(P_ROCKET_SENSE); - int twelve_sense = analogRead(P_TWELVE_SENSE); - int vacuum_sense = analogRead(P_VACUUM_SENSE); - int drive_sense = analogRead(P_DRIVE_SENSE); + int motor5_sense = analogRead(P_MOTOR5_SENSE); + int motor6_sense = analogRead(P_MOTOR6_SENSE); + int motor7_sense = analogRead(P_MOTOR7_SENSE); + int poe_sense = analogRead(P_POE_SENSE); int aux_sense = analogRead(P_AUX_SENSE); - + int multimedia_sense = analogRead(MULTIMEDIA_SENSE); + int nav_sense = analogRead(NAV_SENSE); + int gimbal_sense = analogRead(GIMBAL_ACT_SENSE); + int drive_sense = analogRead(DRIVE_SENSE); + int scisensor_sense = analogRead(SCISENSOR_ACT_SENSE); + int netswitch_sense = analogRead(NETSWITCH_SENSE); + int cam1_sense = analogRead(CAM1_SENSE); + int cam2_sense = analogRead(CAM2_SENSE); + int bbb_sense = analogRead(BBB_SENSE); + int aux_log_sense = analogRead(AUX_LOG_SENSE); + Serial.println("Motor 1 sense: "); Serial.println(motor1_sense); Serial.println("Motor 2 sense: "); @@ -191,16 +200,32 @@ void read_CurrentSense() Serial.println(motor3_sense); Serial.println("Motor 4 sense: "); Serial.println(motor4_sense); - Serial.println("Spare sense: "); - Serial.println(spare_sense); - Serial.println("Rocket sense: "); - Serial.println(rocket_sense); - Serial.println("Twelve sense: "); - Serial.println(twelve_sense); - Serial.println("Vacuum sense: "); - Serial.println(vacuum_sense); - Serial.println("Drive sense: "); - Serial.println(drive_sense); + Serial.println("Motor 5 sense: "); + Serial.println(motor5_sense); + Serial.println("Motor 6 sense: "); + Serial.println(motor6_sense); + Serial.println("Motor 7 sense: "); + Serial.println(motor7_sense); + Serial.println("POE sense: "); + Serial.println(poe_sense); Serial.println("Aux sense: "); Serial.println(aux_sense); -} \ No newline at end of file + Serial.println("Multimedia sense: "); + Serial.println(multimedia_sense); + Serial.println("Nav sense: "); + Serial.println(nav_sense); + Serial.println("Gimbal sense: "); + Serial.println(gimbal_sense); + Serial.println("Drive sense: "); + Serial.println(drive_sense); + Serial.println("SciSensor sense: "); + Serial.println(scisensor_sense); + Serial.println("Cam 1 sense: "); + Serial.println(cam1_sense); + Serial.println("Cam 2 sense: "); + Serial.println(cam2_sense); + Serial.println("BBB sense: "); + Serial.println(bbb_sense); + Serial.println("Aux Log sense: "); + Serial.println(Aux_Log_sense); +} From 739cfee50ae980f1847919361ae769261b8ded79 Mon Sep 17 00:00:00 2001 From: Dakota Miller Date: Fri, 28 Jan 2022 23:38:13 -0600 Subject: [PATCH 03/14] save before computer ded --- powerboardcode/powerboard/powerboard.h | 17 ++++++++--------- powerboardcode/powerboard/powerboard.ino | 2 +- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/powerboardcode/powerboard/powerboard.h b/powerboardcode/powerboard/powerboard.h index cafd733..714a124 100644 --- a/powerboardcode/powerboard/powerboard.h +++ b/powerboardcode/powerboard/powerboard.h @@ -39,6 +39,7 @@ #define P_POE_SENSE PD5 #define P_AUX_SENSE PK1 + //PACK BUSSES #define P_MOTOR1_CTL PC4 #define P_MOTOR2_CTL PC5 @@ -53,15 +54,13 @@ #define MOTOR_DELAY 5000 #define DRIVE_DELAY 5000 -uint8_t currentSense12V[4] = { AUX_MOUNT_SENSE, MULTIMEDIA_SENSE, GIMBAL_SENSE, LOW_CURRENT_SENSE}; -uint8_t actuation12V[3] = { GIMBAL_ACT_CTL, MULTIMEDIA_ACT_CTL, AUX_ACT_CTL}; -uint8_t logic12V[7] = {GIMBAL_LOG_CTL, MULTIMEDIA_LOG_CTL, AUX_LOG_CTL, DRIVE_CTL, NAV_BOARD_CTL, CAM_CTL, EXTRA_CTL}; -uint8_t drivePack = P_DRIVE_CTL; -uint8_t currentSensePack[10] = { P_MOTOR1_SENSE, P_MOTOR2_SENSE, P_MOTOR3_SENSE, P_MOTOR4_SENSE, P_SPARE_SENSE, P_ROCKET_SENSE, P_TWELVE_SENSE, P_VACUUM_SENSE, P_DRIVE_SENSE, P_AUX_SENSE}; -uint8_t bussesPackStart[3] = { P_ROCKET_CTL, P_TWELVE_CTL, P_AUX_CTL}; -uint8_t bussesPack[4] = { P_TWELVE_CTL, P_ROCKET_CTL, P_AUX_CTL, P_DRIVE_CTL}; -uint8_t vacuumCtrl[1] = {P_VACUUM_CTL}; -uint8_t bussesMotor[5] = {P_MOTOR1_CTL, P_MOTOR2_CTL, P_MOTOR3_CTL, P_MOTOR4_CTL, P_SPARE_CTL}; +uint8_t currentSense12V[10] = {MULTIMEDIA_SENSE, NAV_SENSE, GIMBAL_ACT_SENSE, DRIVE_SENSE, SCISENSOR_ACT_SENSE, NETSWITCH_SENSE, CAM1_SENSE, CAM2_SENSE, BBB_SENSE, AUX_LOG_SENSE}; +uint8_t actuation12V[] = {GIMBAL_ACT_CTL, AUX_LOG_CTL, SPARE_CTL, MULTIMEDIA_CTL, SCISENSOR_ACT_CTL}; +uint8_t logic12V[] = {GIMBAL_LOG_CTL, NAV_CTL, DRIVE_CTL, NETSWITCH_CTL, CAM1_CTL, CAM2_CTL, BBB_CTL, SPARE_CTL}; +//uint8_t drivePack = P_DRIVE_CTL; +uint8_t currentSensePack[9] = { P_MOTOR1_SENSE, P_MOTOR2_SENSE, P_MOTOR3_SENSE, P_MOTOR4_SENSE, P_MOTOR5_SENSE, P_MOTOR6_SENSE, P_MOTOR7_SENSE, P_POE_SENSE, P_AUX_SENSE}; +uint8_t bussesPack[2] = {P_AUX_CTL, P_POE_CTL}; +uint8_t bussesMotor[7] = {P_MOTOR1_CTL, P_MOTOR2_CTL, P_MOTOR3_CTL, P_MOTOR4_CTL, P_MOTOR5_CTL, P_MOTOR6_CTL, P_MOTOR7_CTL}; RoveCommEthernet RoveComm; rovecomm_packet packet; diff --git a/powerboardcode/powerboard/powerboard.ino b/powerboardcode/powerboard/powerboard.ino index 1a94649..f69ffef 100644 --- a/powerboardcode/powerboard/powerboard.ino +++ b/powerboardcode/powerboard/powerboard.ino @@ -18,7 +18,7 @@ void loop() { case RC_POWERBOARD_MOTORBUSENABLE_DATA_ID: Serial.println("Enable/Disable Motor Busses"); - for(int i = 0; i < 5; i++) + for(int i = 0; i < 8; i++) //way i<7 { if (packet.data[0] & 1< Date: Sat, 29 Jan 2022 00:34:40 -0600 Subject: [PATCH 04/14] CLOSED DA THING --- powerboardcode/powerboard/powerboard.h | 10 +++++----- powerboardcode/powerboard/powerboard.ino | 21 +++------------------ 2 files changed, 8 insertions(+), 23 deletions(-) diff --git a/powerboardcode/powerboard/powerboard.h b/powerboardcode/powerboard/powerboard.h index 714a124..20c1d29 100644 --- a/powerboardcode/powerboard/powerboard.h +++ b/powerboardcode/powerboard/powerboard.h @@ -54,12 +54,12 @@ #define MOTOR_DELAY 5000 #define DRIVE_DELAY 5000 -uint8_t currentSense12V[10] = {MULTIMEDIA_SENSE, NAV_SENSE, GIMBAL_ACT_SENSE, DRIVE_SENSE, SCISENSOR_ACT_SENSE, NETSWITCH_SENSE, CAM1_SENSE, CAM2_SENSE, BBB_SENSE, AUX_LOG_SENSE}; -uint8_t actuation12V[] = {GIMBAL_ACT_CTL, AUX_LOG_CTL, SPARE_CTL, MULTIMEDIA_CTL, SCISENSOR_ACT_CTL}; -uint8_t logic12V[] = {GIMBAL_LOG_CTL, NAV_CTL, DRIVE_CTL, NETSWITCH_CTL, CAM1_CTL, CAM2_CTL, BBB_CTL, SPARE_CTL}; +uint8_t currentSense12V[] = {MULTIMEDIA_SENSE, NAV_SENSE, GIMBAL_ACT_SENSE, DRIVE_SENSE, SCISENSOR_ACT_SENSE, NETSWITCH_SENSE, CAM1_SENSE, CAM2_SENSE, BBB_SENSE, AUX_LOG_SENSE}; +uint8_t actuation12V[5] = {GIMBAL_ACT_CTL, AUX_LOG_CTL, SPARE_CTL, MULTIMEDIA_CTL, SCISENSOR_ACT_CTL}; +uint8_t logic12V[8] = {GIMBAL_LOG_CTL, DRIVE_CTL, NETSWITCH_CTL, NAV_CTL, CAM1_CTL, CAM2_CTL, BBB_CTL, SCISENSOR_LOG_CTL}; //uint8_t drivePack = P_DRIVE_CTL; -uint8_t currentSensePack[9] = { P_MOTOR1_SENSE, P_MOTOR2_SENSE, P_MOTOR3_SENSE, P_MOTOR4_SENSE, P_MOTOR5_SENSE, P_MOTOR6_SENSE, P_MOTOR7_SENSE, P_POE_SENSE, P_AUX_SENSE}; -uint8_t bussesPack[2] = {P_AUX_CTL, P_POE_CTL}; +uint8_t currentSensePack[9] = {P_MOTOR1_SENSE, P_MOTOR2_SENSE, P_MOTOR3_SENSE, P_MOTOR4_SENSE, P_MOTOR5_SENSE, P_MOTOR6_SENSE, P_MOTOR7_SENSE, P_POE_SENSE, P_AUX_SENSE}; +uint8_t bussesPack[4] = {P_POE_CTL, , P_AUX_CTL, }; uint8_t bussesMotor[7] = {P_MOTOR1_CTL, P_MOTOR2_CTL, P_MOTOR3_CTL, P_MOTOR4_CTL, P_MOTOR5_CTL, P_MOTOR6_CTL, P_MOTOR7_CTL}; RoveCommEthernet RoveComm; diff --git a/powerboardcode/powerboard/powerboard.ino b/powerboardcode/powerboard/powerboard.ino index f69ffef..6b04020 100644 --- a/powerboardcode/powerboard/powerboard.ino +++ b/powerboardcode/powerboard/powerboard.ino @@ -18,7 +18,7 @@ void loop() { case RC_POWERBOARD_MOTORBUSENABLE_DATA_ID: Serial.println("Enable/Disable Motor Busses"); - for(int i = 0; i < 8; i++) //way i<7 + for(int i = 0; i < 7; i++) //way i<7 { if (packet.data[0] & 1< Date: Fri, 25 Feb 2022 16:44:15 -0600 Subject: [PATCH 05/14] restructured folders and naming --- PowerBoard_Software/PowerBoard_Software.h | 154 +++++++ PowerBoard_Software/PowerBoard_Software.ino | 445 ++++++++++++++++++++ PowerBoard_Software_Rev2.zip | Bin 7712 -> 0 bytes powerboardcode/powerboard/powerboard.h | 67 --- powerboardcode/powerboard/powerboard.ino | 216 ---------- 5 files changed, 599 insertions(+), 283 deletions(-) create mode 100644 PowerBoard_Software/PowerBoard_Software.h create mode 100644 PowerBoard_Software/PowerBoard_Software.ino delete mode 100644 PowerBoard_Software_Rev2.zip delete mode 100644 powerboardcode/powerboard/powerboard.h delete mode 100644 powerboardcode/powerboard/powerboard.ino diff --git a/PowerBoard_Software/PowerBoard_Software.h b/PowerBoard_Software/PowerBoard_Software.h new file mode 100644 index 0000000..10a8be9 --- /dev/null +++ b/PowerBoard_Software/PowerBoard_Software.h @@ -0,0 +1,154 @@ +//Program: PowerBoard_Software_Functions.h +//Programmer: Evan Hite +//Purpose: To hold all constants and function prototypes for +// PowerBoard_Software.ino and PowerBoard_Software_Functions.cpp + +#ifndef POWERBOARD_SOFTWARE_FUNCTIONS_HEADER_H +#define POWERBOARD_SOFTWARE_FUNCTIONS_HEADER_H +#include "RoveComm.h" +#include +using namespace std ; + +RoveCommEthernetUdp RoveComm; + +//Struct For Motor Busses, Below are Global Constants +struct PB_Bus +{ + uint8_t rovecomm_cell ; //Bit position in RoveComm packet, either 0 for ACL, or 1 for Motor Busses + uint8_t bit_code_off ; //Binary code with 0 being the position in which the bus exists in the rovecomm packet cell. + uint8_t bit_code_on ; //Binary code with 1 being the position in which the bus exists in the rovecomm packet cell. + int imeas_pin ; //current measurment pin on the board + int ctl_pin ; //control pin on the tiva + int amp_threshold ; //Amp Threshold on the bus + int bus_tuning ; //Tuning for each bus current reading + String identity ; //Identity of the bus +//Basically a constructor, but I do not need it for that purpose +//I am simply wanting to be able to adjust all the values at once +void Adjust_Values (const uint8_t & Rovecomm_cell, const uint8_t & Bit_code_off, const uint8_t & Bit_code_on, const int & Imeas_pin, const int & Ctl_pin, const int & Amp_threshold, const int & Bus_tuning, const String Identity) ; +} ; + +//Global Constants + +//Delays +#define ROVER_POWER_RESET_DELAY 3000 //Delay to reset rover power +#define ROVECOMM_DELAY 10 //Delay to send Rovecomm a package +#define DEBOUNCE_DELAY 10 //Delay after current or voltage trip +#define ROVECOMM_UPDATE_DELAY 1000 //Delay so that Rovecomm is not overloaded +#define COMM_OFF_DELAY 5000 //Delay so that comms turn on after being turned off +#define MOTOR_DELAY 5000 //Delay so that the motors turn on after everything else +#define ROCKET_OFF_DELAY 5000 //Delay so that rockets turn on after being turned off + +//////////////////////////////////////////////Pinmap +// Control Pins for Busses +#define ACT_CTL_PIN PN_3 //In rev 2 +#define LOGIC_CTL_PIN PD_1 //In rev 2 +#define COMM_CTL_PIN PH_2 //In rev 2 +#define COMM_LOGIC_CTL_PIN PP_2 //In rev 2 +#define EM_CTL_PIN PK_5 //Was Auxillary +#define FM_CTL_PIN PK_7 //Was M1 +#define MM_CTL_PIN PK_6 //Was M2 +#define BM_CTL_PIN PH_1 //Was M3 +#define AUX_CTL_PIN PH_0 //Was M4 +#define ROCKET_CTL_PIN PM_2 //Was M7 +#define GE_CTL_PIN PM_1 //Was M6 +//#define M7_CTL_PIN PM_0 //No longer in use +#define FAN_CTL_PIN PM_3 //In rev 2 + +// Sensor Volts/Amps Readings Pins +#define ACT_I_MEAS_PIN PE_2 //In rev 2 +#define LOGIC_I_MEAS_PIN PE_1 //In rev 2 +#define COMM_I_MEAS_PIN PE_0 //In rev 2 +#define EM_I_MEAS_PIN PD_0 //Was Auxillary in rev 1 +#define FM_I_MEAS_PIN PK_3 //was M1 +#define MM_I_MEAS_PIN PK_2 //was M2 +#define BM_I_MEAS_PIN PK_1 //was M3 +#define AUX_I_MEAS_PIN PK_0 //was M4 +#define ROCKET_I_MEAS_PIN PB_5 //Was M5 +#define GE_I_MEAS_PIN PB_4 //Was M6 +//#define M7_I_MEAS_PIN PE_3 //No longer in use +//#define PACK_VOLTAGE_PIN PE_5 //No longer in use + +//////////////////////////////////////////////RoveBoard +// Tiva1294C RoveBoard Specs +#define VCC 3300 //volts +#define ADC_MAX 4096 //bits +#define ADC_MIN 420 //meme bits + +//////////////////////////////////////////////Sensor +// ACS722LLCTR-40AU-T IC Sensor Specs +#define SENSOR_SENSITIVITY 0.066 //volts/amp +#define SENSOR_SCALE 0.1 //volts/amp +#define SENSOR_BIAS VCC * SENSOR_SCALE + +#define CURRENT_MAX 40000//((VCC - SENSOR_BIAS) / SENSOR_SENSITIVITY) +#define CURRENT_MIN 0//-SENSOR_BIAS / SENSOR_SENSITIVITY + +#define VOLTS_MIN 0 +#define VOLTS_MAX 33600 + +//Safest Test pin +#define ESTOP_12V_COMM_LOGIC_MAX_AMPS_THRESHOLD 5000 //5 amps, 5 amp fuse +#define ESTOP_12V_ACT_MAX_AMPS_THRESHOLD 15000 //15 amps, 20 amp fuse +#define ESTOP_AUX_MAX_AMPS_THRESHOLD 21000 //19 amps, 20 amp fuse(360W/21.6) = 16.6 Amps, due to inaccurate readings raised to 19 +#define ESTOP_MOTOR_BUS_MAX_AMPS_THRESHOLD 30000 //30 amps, 40 amp fuse +#define ESTOP_ROCKET_BUS_MAX_AMPS_THRESHOLD 4000 //4 amps, 5 amp fuse +#define ESTOP_GEN_EXTRA_BUS_MAX_AMPS_THRESHOLD 30000 // 30 Amps, 40 Amp fuse + +//Tuning Variables +#define LOGIC_COMM_TUNER 1010 //Tuner for ADC values for Logic and Comm busses +#define ACT_TUNER 1000 //Tuner for ADC values for Actuation bus +#define AUX_TUNER 1020 //Tuner for ADC values for Auxilliary bus +#define MOTOR_TUNER 1005 //Tuner for ADC values for motor busses +#define ROCKET_TUNER 1005 //Tuner for ADC values for Rocket antennas +#define CURRENT_AVERAGE 10 //The amount of current readings that we average over to get accurate current readings + +//Number of Busses +#define ALC_BUSSES 3 //Number of Actuation, logic and Communication busses +#define MOTOR_BUSSES 7 //Number of busses besides actuation, logic, and communication + +//Functions///////////////////////////////////////////////////////////////////////// + +//Description: Checks the pin for bouncing voltages to avoid false positive +//Pre: Bouncing_pin is a pin on the tiva that reads amperage, max_amps_threshold +// is the maximum aperage the device can tolerate +//Post: Returns a bool of true if the pin has too much current +bool singleDebounce(const int & bouncing_pin,const int & max_amps_threshold, const int & Tuner); + +//Description: Sets the pins on the tiva to certain buses on the powerboard +//Pre: None +//Post: All pins on the tiva are set to correct values. +void Configure_Pins (); + +//Description: Turns all pins to on after setting all to low +//Pre: Pins must be in correct position from Configure_Pins +//Post: All pins are set high +void Pin_Initialization (); + +//Description: Begins communication with RoveComm and sends a packet to rovecomm +//Pre: Bus[] must have a size of RC_POWERBOARD_BUSENABLED_DATACOUNT to work. +//Post: Sends to RoveComm a packet that the powerboard is live. +void Communication_Begin (uint8_t Bus []) ; + +//Description: Checks overcurrent on the Bus and shuts off in need be +//Pre: BUS_I_MEAS_PIN must be a pin for current measurement, Bus[] must be the same array from Commuincation_Begin. +// BUS_CTL_PIN must be the relevent pin that controls the bus. and ESTOP_AMP_THRESHOLD must also be the relevent estop threshold. +//Post: Shuts off the bus if there is an overcurrent situation and writes this to Bus[] +bool Shut_Off(const PB_Bus & Bus, uint8_t Send_Recieve[], const uint16_t & Current_Reading) ; + +//Description: Takes a packet from RoveComm and turn on or off a bus +//Pre: Enable_Disable should be a packet recieved from RoveComm, and Bus should be from Shut_Off +//Post: Turns on or off all buses that the packet says should be on or off and writes this to Bus[]. +void Bus_Enable (const rovecomm_packet & Enable_Disable, uint8_t Send_Recieve[], const PB_Bus Bus[]) ; + +//Description: Reads a current sensing pin and saves this value to current_reading +//Pre: Current_Reading should be an array part, for RoveComm reasons. BUS_I_MEAS_PIN should be a current measuring pin +//that corresponds to the spot on the Current_Reading array. +//Post: Returns a value to current_reading of the current reading value from the BUS_I_MEAS_PIN in mA. +void Pin_Read (uint16_t & current_reading, const int & BUS_I_MEAS_PIN, const int & Tuner) ; + +//Description: +//Pre: +//Post: +void Bus_Setup(PB_Bus Bus[]) ; + +#endif diff --git a/PowerBoard_Software/PowerBoard_Software.ino b/PowerBoard_Software/PowerBoard_Software.ino new file mode 100644 index 0000000..cf52889 --- /dev/null +++ b/PowerBoard_Software/PowerBoard_Software.ino @@ -0,0 +1,445 @@ +//RoveWare Powerboard ACS_722 Interface +// +// Created for Zenith by: Judah Schad, jrs6w7 +// Altered for Gryphon by: Jacob Lipina, jrlwd5 +// Altered for Valkyrie by: Evan Hite erhtpc +// Using http://www.digikey.com/product-detail/en/allegro-microsystems-llc/ACS722LLCTR-40AU-T/620-1640-1-ND/4948876 +// +// Standard C +#include +#include +#include + +//Local Files +#include "PowerBoard_Software_Functions_Header.h" + +//Bus Control Variables +uint8_t Send_Recieve[] {0,0} ; //Bus to Enable or Disable +PB_Bus Bus[RC_POWERBOARD_IMEASmA_DATACOUNT] ; //An array of all the Busses on the Powerboard + +//Packet Reception and Sent Variables +rovecomm_packet Enable_Disable ; //packet reception variable +uint16_t Current_Reading[RC_POWERBOARD_IMEASmA_DATACOUNT] ; //Current Reading for all busses +uint16_t error_reading[CURRENT_AVERAGE][RC_POWERBOARD_IMEASmA_DATACOUNT] ; +bool Bus_Tripped ; //To determine whether or not to send a packet based on overcurrents +bool sent_packet = true ; //To determine whether or not to send a packet of current values +uint32_t last_time_packet = 0 ; //Time since last packet or current values sent +bool Overcurrent = false ; //Shows whether or not a bus has overcurrented +int times_through = 0 ; +int average_holder = 0 ; +uint32_t comm_off_timer = 0 ; +bool comm_off = false ; +uint32_t Motor_Turn_On_Time = 0 ; +bool rockets_off = false ; +uint32_t rocket_off_timer = 0 ; + +//////////////////////////////////////////////Powerboard Begin +// the setup routine runs once when you press reset +void setup() +{ + Serial.begin(115200) ; + delay(500) ; +//Serial.println("Got here...") ; + Configure_Pins () ; //Configures pins to correct busses + Pin_Initialization () ; //Sets pins to low then to high + Bus_Setup(Bus) ; //Sets up the PowerBoard Busses in software + Communication_Begin (Send_Recieve) ; //Sends to base station that everything is now on and communication begins + Motor_Turn_On_Time = millis()+MOTOR_DELAY ; +}//end setup + +/////////////////////////////////////////////Powerboard Loop Forever +void loop() +{ + //Current Readings to Report back to Base Station////////////////////////////////////////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + //Code to setup sending a current reading packet every second + static bool first_run = true; + if(millis() >= Motor_Turn_On_Time && first_run) + { + Serial.println("Motor On"); + first_run = false; + digitalWrite(EM_CTL_PIN, HIGH) ; + digitalWrite(FM_CTL_PIN, HIGH) ; + digitalWrite(MM_CTL_PIN, HIGH) ; + digitalWrite(BM_CTL_PIN, HIGH) ; + } + if(sent_packet == true) + { + last_time_packet = millis() ; //Timestamp + sent_packet = false ; //So last_time_packet will not be overwritten too quickly + } + for(int i = 0 ; i < (RC_POWERBOARD_IMEASmA_DATACOUNT) ; i++) + { + Pin_Read(Current_Reading[i], Bus[i].imeas_pin, Bus[i].bus_tuning) ; + //Serial.print(Bus[i].identity) ; + //Serial.println(" Current Reading") ; + //delay(10) ; + } + for(int k = 0 ; k <(RC_POWERBOARD_IMEASmA_DATACOUNT) ; k++) + { + error_reading[times_through][k] = Current_Reading[k] ; + } + times_through++ ; + //Serial.println("") ; + //End of Current Reads + //Sends Current Values back to basestation every second, after the board has run through the code once + if(millis() >= (last_time_packet+ROVECOMM_UPDATE_DELAY)) + { + RoveComm.write(RC_POWERBOARD_IMEASmA_DATAID, RC_POWERBOARD_IMEASmA_DATACOUNT, Current_Reading) ; //Sends back current readings + delay(ROVECOMM_DELAY) ; + RoveComm.write(RC_POWERBOARD_BUSENABLED_DATAID, RC_POWERBOARD_BUSENABLED_DATACOUNT, Send_Recieve) ; //Sends back present state of motor busses + sent_packet = true ; //So we can update + delay(ROVECOMM_DELAY) ; +//Serial.println("Hello") ; //Debug Code +//delay(10) ; + } + + //Checking for Over Currents on all busses////////////////////////////////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +//Serial.println("Checking comms") ; //Serial Debugging Code +//delay(10) ; + if(times_through >= CURRENT_AVERAGE) + { + times_through = 0 ; //Reset the averaging of the current reads + for(int L = 0 ; L < (RC_POWERBOARD_IMEASmA_DATACOUNT) ; L++) //Averaging the Current Readings over the amount specified by the Current_Average + { + for(int M = 0 ; M < CURRENT_AVERAGE ; M++) + { + average_holder = average_holder + error_reading[M][L] ; + } + average_holder = average_holder/CURRENT_AVERAGE ; + Current_Reading[L] = average_holder ; + average_holder = 0 ; + //Serial.print(Bus[L].identity) ; + //Serial.print(" current reading of ") ; + //Serial.print(Current_Reading[L]) ; + //Serial.println(" mA") ; + } + for(int j = 0 ; j < (RC_POWERBOARD_IMEASmA_DATACOUNT) ; j++) + { + Bus_Tripped = Shut_Off(Bus[j], Send_Recieve, Current_Reading[j]) ; +// Serial.print(Current_Reading[j]) ; +// Serial.print(" ") ; +// Serial.print(Bus[j].identity) ; +// Serial.println(" Overcurrent check") ; +// delay(10) ; + if(Bus_Tripped == true) + { + Overcurrent = true ; + Serial.print(Bus[j].identity) ; + Serial.print(" Overcurrented at") ; + Serial.print(Current_Reading[j]) ; + Serial.println("mA") ; + } + } + //Serial.println("") ; + //End of Overcurrents + //If any bus senses an overcurrent, then a packet will be sent + if(Overcurrent == true) + { + RoveComm.write(RC_POWERBOARD_BUSENABLED_DATAID, RC_POWERBOARD_BUSENABLED_DATACOUNT, Send_Recieve) ; //Send out a summary of what is off after current check + delay(ROVECOMM_DELAY) ; + Bus_Tripped = false ; + Overcurrent = false ; + } + } + /////////////////////////////////////////////RED Control and Telem RoveComm + //Recieves Pack form rovecomm and shuts off or turns on busses at need + Enable_Disable = RoveComm.read(); + if(Enable_Disable.data_id == RC_POWERBOARD_BUSENABLE_DATAID) + { + Bus_Enable(Enable_Disable, Send_Recieve, Bus) ; + //Serial.println("") ; + //Serial.println("Packet Recieved") ; + //Serial.println("") ; + delay(500) ; //Delay half a second after recieving a packet to let the rover turn on and off certain busses. + if(digitalRead(COMM_CTL_PIN) == LOW) + { + comm_off = true ; + comm_off_timer = millis() ; + } + if(digitalRead(ROCKET_CTL_PIN) == LOW) + { + rockets_off = true ; + rocket_off_timer = millis() ; + } + } + if(comm_off == true) + { + if(millis() >= (comm_off_timer+COMM_OFF_DELAY)) + { + digitalWrite(COMM_CTL_PIN, HIGH) ; + comm_off = false ; + delay(1000) ; //Delay a second to let everything turn on + } + } + if(rockets_off == true) + { + if(millis() >= (rocket_off_timer+ROCKET_OFF_DELAY)) + { + digitalWrite(ROCKET_CTL_PIN, HIGH) ; + rockets_off = false ; + delay(1000) ; //Delay a second to let it turn on + } + } +} +//End of Main Loop + + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +//Functions +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + +//Function 1.//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool singleDebounce(const int & bouncing_pin, const int & max_amps_threshold, const int & Tuner) +{ + int adc_threshhold = ((map(max_amps_threshold, CURRENT_MIN, CURRENT_MAX, ADC_MIN, ADC_MAX)*1000)/(Tuner)); //Get reading off pin + bool trip = false ; + if(analogRead(bouncing_pin) > adc_threshhold) //If pin reading is high + { + delay(DEBOUNCE_DELAY); + if( analogRead(bouncing_pin) > adc_threshhold) //If pin reading is still high + { + trip = true; //Sends back true to indicate shut off + }//end if + else + { + trip = false ; + } + }// end if + return trip; +} + +//Function 2.//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void Configure_Pins () +{ + // Control Pins are outputs + pinMode(ACT_CTL_PIN, OUTPUT); + pinMode(COMM_LOGIC_CTL_PIN, OUTPUT); + pinMode(COMM_CTL_PIN, OUTPUT); + pinMode(LOGIC_CTL_PIN, OUTPUT); + pinMode(EM_CTL_PIN, OUTPUT); + pinMode(FM_CTL_PIN, OUTPUT); + pinMode(MM_CTL_PIN, OUTPUT); + pinMode(BM_CTL_PIN, OUTPUT); + pinMode(AUX_CTL_PIN, OUTPUT); + pinMode(ROCKET_CTL_PIN, OUTPUT); + pinMode(GE_CTL_PIN, OUTPUT); + pinMode(FAN_CTL_PIN, OUTPUT); + + //Current Measurement pins are inputs + pinMode(ACT_I_MEAS_PIN, INPUT); + pinMode(COMM_I_MEAS_PIN, INPUT); + pinMode(LOGIC_I_MEAS_PIN, INPUT); + pinMode(EM_I_MEAS_PIN, INPUT); + pinMode(FM_I_MEAS_PIN, INPUT); + pinMode(MM_I_MEAS_PIN, INPUT); + pinMode(BM_I_MEAS_PIN, INPUT); + pinMode(AUX_I_MEAS_PIN, INPUT); + pinMode(ROCKET_I_MEAS_PIN, INPUT); + pinMode(GE_I_MEAS_PIN, INPUT); + return ; +} + +//Function 4.//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void Pin_Initialization () +{ + digitalWrite(ACT_CTL_PIN, LOW); + digitalWrite(COMM_LOGIC_CTL_PIN, LOW); + digitalWrite(COMM_CTL_PIN, LOW); + digitalWrite(LOGIC_CTL_PIN, LOW); + digitalWrite(EM_CTL_PIN, LOW); + digitalWrite(FM_CTL_PIN, LOW); + digitalWrite(MM_CTL_PIN, LOW); + digitalWrite(BM_CTL_PIN, LOW); + digitalWrite(AUX_CTL_PIN, LOW); + digitalWrite(ROCKET_CTL_PIN, LOW); + digitalWrite(GE_CTL_PIN, LOW); + digitalWrite(FAN_CTL_PIN, LOW); + // Turn on everything when we begin + delay(ROVER_POWER_RESET_DELAY); //Three Second Delay + + //After Delay, make sure everything turns on + digitalWrite(ACT_CTL_PIN, HIGH); + digitalWrite(COMM_LOGIC_CTL_PIN, HIGH); + digitalWrite(COMM_CTL_PIN, HIGH); + digitalWrite(LOGIC_CTL_PIN, HIGH); + //digitalWrite(EM_CTL_PIN, HIGH); + //digitalWrite(FM_CTL_PIN, HIGH); + //digitalWrite(MM_CTL_PIN, HIGH); + //digitalWrite(BM_CTL_PIN, HIGH); + digitalWrite(AUX_CTL_PIN, HIGH); + digitalWrite(ROCKET_CTL_PIN, HIGH); + digitalWrite(GE_CTL_PIN, HIGH); + digitalWrite(FAN_CTL_PIN, HIGH); //Fans Always On + return ; +} + +//Function 5.///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void Communication_Begin (uint8_t Send_Recieve []) //This function may have to change +{ + RoveComm.begin(RC_POWERBOARD_FOURTHOCTET); + delay(100) ; + Serial.println("Setting Up..."); + delay(ROVECOMM_DELAY); + uint8_t ALC_Bits = 1 ; + uint8_t Motor_Bits = 1 ; + uint8_t binary_hold = 2 ; + for(int i = 1 ; i < ALC_BUSSES ; i++) + { + ALC_Bits = ALC_Bits + binary_hold ; + binary_hold = 2*binary_hold ; + } + binary_hold = 2 ; + for(int i = 1 ; i < MOTOR_BUSSES ; i++) + { + Motor_Bits = Motor_Bits + binary_hold ; + binary_hold = 2*binary_hold ; + } + Send_Recieve[0] = ALC_Bits ; + Send_Recieve[1] = Motor_Bits ; + RoveComm.write(RC_POWERBOARD_BUSENABLED_DATAID, RC_POWERBOARD_BUSENABLED_DATACOUNT, Send_Recieve) ; //Initial states of Busses +} + +//Function 6.//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool Shut_Off( const PB_Bus & Bus, uint8_t Send_Recieve[], const uint16_t & Current_Reading) +{ + bool Bus_Tripped = false ; + //Special Variables in case of the communication bus being tripped + static uint32_t time1 = 0 ; + static bool comms_off = false ; + const uint8_t comm_on_bit_code = 4 ; + if(Current_Reading > Bus.amp_threshold ) //If pin is tripped + { + Bus_Tripped = true ; + //Special Case for the Communication Bus + if(Bus.ctl_pin == COMM_CTL_PIN)//Special rules for communication bus + { + Send_Recieve[0] = Send_Recieve[0] & Bus.bit_code_off ; + RoveComm.write(RC_POWERBOARD_BUSENABLED_DATAID, RC_POWERBOARD_BUSENABLED_DATACOUNT, Send_Recieve) ; //Send a packet to rovecomm prior to shutting off comms + delay(ROVECOMM_DELAY) ; + digitalWrite(COMM_CTL_PIN, LOW); //Turn off the comms + if(time1 == 0) //Timing Code, to turn on comms in 10 seconds, first half + { + comms_off = true ; + time1 = millis(); + } + } + else //All other busses + { + Send_Recieve[Bus.rovecomm_cell] = Send_Recieve[Bus.rovecomm_cell] & Bus.bit_code_off ; + //Bitwise and with ones in bit code will result in only changing the specific bit I want changed + digitalWrite(Bus.ctl_pin, LOW) ; //Turning off the bus + delay(ROVECOMM_DELAY) ; //Just adding a delay in case the switch takes longer than expected + } + } + if(comms_off == true) //Second half of timing code to turn on comms after 10 seconds + { + if(millis()>=(time1+COMM_OFF_DELAY)) //it is turned off in case the overcurrent was just a random spike. + { + comms_off = false ; //If there actually is a short in the bus, the bus will turn itself + time1 = 0 ; //back on + digitalWrite(COMM_CTL_PIN,HIGH); + Send_Recieve[0] = Send_Recieve[0] | comm_on_bit_code ; + } + } + return Bus_Tripped ; +} + +//Function 7.////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void Bus_Enable (const rovecomm_packet & Enable_Disable, uint8_t Send_Recieve[], const PB_Bus Bus[]) +{ + int ALC_to_Motor_Count = ALC_BUSSES ; + int Start_Point = 0 ; + int bit_helper = 1 ; + Serial.println("Bus En"); + Serial.println(Enable_Disable.data[0]); + Serial.println(Enable_Disable.data[1]); + Serial.println(Enable_Disable.data[2]); +for(int i = 0 ; i < (RC_POWERBOARD_BUSENABLE_DATACOUNT) ; i++) +{ + for(int j = Start_Point ; j < ALC_to_Motor_Count ; j++) + { + //Serial.println(j) ; + //delay(10) ; + //Serial.print(Enable_Disable.data[i]) ; + //Serial.println( " <- data sent") ; + //delay(10) ; + if(bitRead(Enable_Disable.data[i] , (bit_helper)) == 1) //Changing State of the Bus + { + //Serial.print("Changing State of ") ; + //delay(10) ; + if(Enable_Disable.data[2] == 1) //Turn on + { + Serial.print(Bus[j].identity) ; + Serial.println(" Turned on by user") ; + //delay(10) ; + Send_Recieve[i] = Send_Recieve[i] | Bus[j].bit_code_on ; + digitalWrite(Bus[j].ctl_pin, HIGH) ; + delay(ROVECOMM_DELAY) ; + } + else //Turn OFF + { + Serial.print(Bus[j].identity) ; + Serial.println( " Turned off by user") ; + //delay(10) ; + Send_Recieve[i] = Send_Recieve[i] & Bus[j].bit_code_off ; + digitalWrite(Bus[j].ctl_pin, LOW) ; + delay(ROVECOMM_DELAY) ; + } + } + bit_helper++ ; + } + Start_Point = ALC_to_Motor_Count; + ALC_to_Motor_Count = ALC_to_Motor_Count + MOTOR_BUSSES ; + bit_helper = 1 ; +} + return ; +} + +//Function 8.//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void Pin_Read (uint16_t & current_reading, const int & BUS_I_MEAS_PIN, const int & Tuner) +{ + // Using http://www.digikey.com/product-detail/en/allegro-microsystems-llc/ACS722LLCTR-40AU-T/620-1640-1-ND/4948876 + int adc_reading = analogRead(BUS_I_MEAS_PIN) ; + if(adc_reading < ADC_MIN) + { + adc_reading = ADC_MIN ; + } + if(adc_reading > ADC_MAX) + { + adc_reading = ADC_MAX ; + } + current_reading = ((((map(adc_reading, ADC_MIN, ADC_MAX, CURRENT_MIN, CURRENT_MAX)*1180)/1000)*Tuner)/1000); + //Serial.println(current_reading) ; + return ; +} + +void PB_Bus::Adjust_Values (const uint8_t & Rovecomm_cell, const uint8_t & Bit_code_off, const uint8_t & Bit_code_on, const int & Imeas_pin, const int & Ctl_pin, const int & Amp_threshold, const int & Bus_tuning, const String Identity) +{ + rovecomm_cell = Rovecomm_cell ; + bit_code_off = Bit_code_off ; //Binary converted to decimal (255-bit_code_on) + bit_code_on = Bit_code_on ; //Binary converted to decimal, 2^(place in cell) + imeas_pin = Imeas_pin ; + ctl_pin = Ctl_pin ; + amp_threshold = Amp_threshold ; + bus_tuning = Bus_tuning ; + identity = Identity ; +} +void Bus_Setup(PB_Bus Bus[]) +{ + Bus[0].Adjust_Values(RC_POWERBOARD_BUSENABLE_ALCENTRY, 254, 1, ACT_I_MEAS_PIN, ACT_CTL_PIN, ESTOP_12V_ACT_MAX_AMPS_THRESHOLD, ACT_TUNER, "Actuation") ; + Bus[1].Adjust_Values(RC_POWERBOARD_BUSENABLE_ALCENTRY, 253, 2, LOGIC_I_MEAS_PIN, LOGIC_CTL_PIN, ESTOP_12V_COMM_LOGIC_MAX_AMPS_THRESHOLD, LOGIC_COMM_TUNER, "Logic") ; + Bus[2].Adjust_Values(RC_POWERBOARD_BUSENABLE_ALCENTRY, 251, 4, COMM_I_MEAS_PIN, COMM_CTL_PIN, ESTOP_12V_COMM_LOGIC_MAX_AMPS_THRESHOLD, LOGIC_COMM_TUNER, "Communications") ; + Bus[3].Adjust_Values(RC_POWERBOARD_BUSENABLE_MOTORSENTRY, 254, 1, FM_I_MEAS_PIN, FM_CTL_PIN, ESTOP_MOTOR_BUS_MAX_AMPS_THRESHOLD, MOTOR_TUNER, "Front Motors") ; + Bus[4].Adjust_Values(RC_POWERBOARD_BUSENABLE_MOTORSENTRY, 253, 2, MM_I_MEAS_PIN, MM_CTL_PIN, ESTOP_MOTOR_BUS_MAX_AMPS_THRESHOLD, MOTOR_TUNER, "Middle Motors") ; + Bus[5].Adjust_Values(RC_POWERBOARD_BUSENABLE_MOTORSENTRY, 251, 4, BM_I_MEAS_PIN, BM_CTL_PIN, ESTOP_MOTOR_BUS_MAX_AMPS_THRESHOLD, MOTOR_TUNER, "Back Motors") ; + Bus[6].Adjust_Values(RC_POWERBOARD_BUSENABLE_MOTORSENTRY, 247, 8, EM_I_MEAS_PIN, EM_CTL_PIN, ESTOP_MOTOR_BUS_MAX_AMPS_THRESHOLD, MOTOR_TUNER, "Extra Motors") ; + Bus[7].Adjust_Values(RC_POWERBOARD_BUSENABLE_MOTORSENTRY, 239, 16, ROCKET_I_MEAS_PIN, ROCKET_CTL_PIN, ESTOP_ROCKET_BUS_MAX_AMPS_THRESHOLD, ROCKET_TUNER, "Rockets") ; + Bus[8].Adjust_Values(RC_POWERBOARD_BUSENABLE_MOTORSENTRY, 223, 32, GE_I_MEAS_PIN, GE_CTL_PIN, ESTOP_GEN_EXTRA_BUS_MAX_AMPS_THRESHOLD, MOTOR_TUNER, "General Extra") ; + Bus[9].Adjust_Values(RC_POWERBOARD_BUSENABLE_MOTORSENTRY, 191, 64, AUX_I_MEAS_PIN, AUX_CTL_PIN, ESTOP_AUX_MAX_AMPS_THRESHOLD, AUX_TUNER, "Auxillary") ; +} + + + diff --git a/PowerBoard_Software_Rev2.zip b/PowerBoard_Software_Rev2.zip deleted file mode 100644 index 19f3c1e63daa9cf6f467df9fba0e83402f06dd7a..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 7712 zcmb7pWpEulvgI}1n6H_cV|L8U6f;B2OtBp^(>2D-j4?CC%*@Qp%nY%;GxO%FeY5+$ zty-z1BbC(Mr}|e{A4M4`XbiwVhezX+{J#$W-f;hcigvE14kC7j4kr34c4khlh7P9s z%BC(~m=ynQ2Kz6wjh`-tr0@U$FA)GB`2RHf8-Ehcw#H7DcD9cCQl^F`rVfl26N4Oq~%gq_4(hU3lB8!f-X7(e+0d;hu(p379-d(bd8ZVwV z>kGkT9608T@ET@0%tF)xBNnAe8LudA%1yKZ7)pQ^`}1zUi(>g_Ab^u)k_Cc?4{PWH zX1k|1cm5^GZWn1auK&-kMuLgVeHzViySM0x83fx)^Gd<2N%ii`!-zIh`^6V53-_^i zR_^FtnpNhTab-RhjkCwhbdhh-7?-$@+lyWP;$UA{+vD(>lZOa(0}?8iQR0lsY&*!lN#W5Y4DCePjX%4%Wc(x zHcKHBv9xa4VEcn%j9S^P``Fw`xg@e6kh>NT$03Fi`?Fp7)=H+Gso-A6Olb`XB|$2X$2jErAZu9ol9D7!>kb?CM}+2}7MU%!-k*fhxE7TH>vI_HDoWN)PnTRb zS_@V^04ISg?h*vJM(i#nAnRfctRxSva=JCAQYpu>+NZ`dRm1{_^^sch!*!5Gk--t& z%>|+L4RpJjXd&Nllqi5?w%u(xCa4XOsRjnte;aZ}rh(;`f6SNUI>+Kpo@b~3!AuD9 z4IO#&Fxp)T6~7Z-5&4}VC`75ePpj!cHwlv1=zETg8P%(jMC4Ax3^e5VKzHbB(ek%H zoVA=-uDX)OvMU#dQwtL!PTePR@8!mjxP;|gFRa!ivPVHsAp7c{?AH@=aPt@*yL-s> zR{Nw}%uL(2rQDB8iw@Ufh#y`UbI{lbJb1xD6p@NEgFDDbQcec419K7l4IzpgeLMKT zg#k{{LDk5^LM_QoAx-pg<@wdG-M5_-CS=uHUP@!?k7Zdf(T{h2uVvP>pop$Q>(Qpf znICUm_tf1+eB_DdEC`G>B7G>z3MK})RJV`Dk2s@77Nk>OWt;nY)NY zTe4>{b7{~gp~Qg{nJ6yYN}s`8k4v(Aptjc?G_FxuC0ufw#-ePVeS1Q~yDV*FY>USCT{ zIPWoR?B3x)SCFaDdb#@G8?`?xhetL&x}>wOAp3opEMs?oKZ<1*RJ7XOB($(-x}=fs zDVD6^+?I9@aq0}WMsTiEvs z1;djj4|>9>2L2B(7x%_EHF+Gdc;)p8gsbembVCY?91Ts!AoJKPr{Wud^;Dff;M`Wu z6hRPemhfg*`}ZDQzZc?Gu+QNE=5y+u9S>)p5?i|LI?WatPS59rLi_B|#@IlIVGgcU z-*4y=*-ycbW$|RX5B1E+{N!V5mkGH}a(p2!1vQm;R8{!L?VJd0`fmk}coir}dJ(9V zTa?Kark8;rwr#iFxcLgD!4O};+a$MyklwZ(gRqXbXdOpl6@BWR*pO1xUwFXN0f-sE zKB(!6J`24NxBnZrNFrx0aAJ!fYFUr_tO}E4Ujs=E*2xqOM?=Ql|^t z$5ez^T|0f(kM2+&MX}9?{S(#1i{&GN&sv@{;%f~t8h~{BTBiCPem-nzM(SWapSrLm zcj}q|4>*6Youk{{31*zDj7_;1iTqw$lnq4>DSirQ)UFKdmhPC`0#=sHF9q{oqv*&% zXo)db;#Y|jUT$?oG>Z9sx)#rW;etyL?f6}KPD4vJB!J-R-c*ml(m+ejVf6Kfi;}H= zxlHMA8Fck06>m@?VU>@~l4rlN^>?k`v3@S!;*=``!uR7_Jah4^n%m9E7%^P)iv0^# z@~1u&Qy|%pRN`|8=aqU^uaX4KrUca=x5LQ;URE-IgfV9o1hF--LGfJ3-pe{)f*J;W z7SjAlZ~DXEcR5gBS5A=R%I|pJn`Unyb(y0e0wn%0B@#)R64gp>RUO1y1+Q>09i*OI zOU`4pA&($WeSOF7E!J)ko~9oRd^DS|--(2@IN)Ne5BBQw#2PM& zQnLv-T-dd8!SD}<07&kDAve>01k@(AmR8S6Ez|i_bdG+R*3o6 zH0?pc+MGRQwY*-E$R23+u`RehsZy03`yq)YB{RKF9YD zDn)ONb}Iuss6m~JS%q>~YtyT*YpWY+QVCHQ>|^{F{ce2l5;{B75WOa-5@Pk;%;vfte{!s^ig(B=UPTU5bh-n6 zevltr@cGTxOf!?)@zwOQ!k=(8IlYq2c8Hx0(h7!_h!M~KilGl-v*8`)Kda9uz%$4f z6#%%C1OV9of2+@bR?)3Q-k5&)-E{e@^!)$ zCJ<;ji1^n2B*ML5InY;=b5DMZ+6FK14Ev(>I%1vQr?*3fY9az@kkwq-{IciB?HMh1 zm1rz!>TrA42Pq;-J{=w7;BN%#6ASgtnYdQh$)Uy)(|`0%>yQBYCm;-h?X?TeF` z=w*gISIoQEKm;z#57~YgAgvrOoMTz5O=&7BKhQW~BM9W~OG2dE+jljeIyPkdh|v#S zxd!9RWBx!%>ZQ1Km4}}7b4AhM!U~jxw1d`9ypz`?p!%~r4HltPMyX7ey6EJAzTl_+ zdA^kC*goK|3COB}Jxf8C{YguoLk0;`@|NHx#_HTAHN}$wGg>T_5B_PuNj(Ku~xD$;t4TjuHB&3(}zf4Tr-_efqbt?$3}2#5&g{J4O@yWG+9*mvj)arWUTO*el| zCb%s=Vf5oi4j{{Q6|*^CqQmMgGNM2rFnS>WUoB$?W_Ycw9!5xJegj)7PU2}pFYof_>`|wmX>q~^Q|S7m9*tqSb>~~^ zTYqJFd;d@~w_}>vT+oD@TILW+UIjRR_0I}KaY#=_Q5Y$2zPE6db|pc%^AeuIQ!{jg zlNFN@5=|c?ELt~(OBle=g?#Ybi6U;36eg#FQCri}L2cf?#8+Rh-d{wqwNqdr3m|yW z{GC$N)aV=`f7J*v#~q7w&FJlr+=Z3=1Sr}|1NE!1J3`W|KPib~Rs>=acuRtRDsJ{j z>2Ey=+avmJ+VozD25*auB&=50`Fc0OjKN5E^(OccJZ%v1bGomT3_~F7m@tpmz)q_N zUEDo`5+W3JRJL^_ZZj?yQdxt_!x~WF^Ah}XM5O~POc1;M8?@!6D^;MFgU0v?V2h$xjH>7&xp1g54BpROTR zl(`g{XBJ{q0;j~%Udsz(;;`~j=6jWXJ0d#pm>QUkq0ajS=|D_HjRXyCRrQS*uo?w?{)pN=G75 z8%$9myV29_Knl@bZVx{@kpPyRq=g#un|ADn^Iw23d=h#EN8s}S9`^1wl1Mv0p`s`Z zTfvCwN3`uQlhdSrg42G6Tr)W7Ilj1H;qAq8WA35ZV6#jPV0(Hkq=EOM75U*p#u_Ej z|H(5b$J_nY`cgd@Q2$&}&4$od{M=s*T^!l`x!gVUS1u;L!Q>J^t|>Dkso=&{Js`ag zehM@Fv@J8wV+41Kx1_-J8{QP}O!|U$g9)HGxQ?RXb@p@3+XV-)cw`p{g=_>)l^ZAs z=@fEaj)(4Kah-0jE!F(%1IJhx`TNB7wy1peu3n&fZ@Hrz3}Wt5)XJA8zKZI4beVfw z1}O*&cz;B3&xDuDv@?lAPOUsLZU>g2-2-ylFHPh-sEfmxp(i^-XVtO`;kZoam6)IR z5%m~MDR)0yKFoy!OtWcHCo^M8BV_o@TT#6-N80GkMTNFo2sfO_baPqCC8G;|y1y)a zcQ(i@g=D~llvvxnRrL#$`$EVd5hEUI~iO_KKU&yu;+?9o5a6g zM7)Ds zn#m0!=`vpzDsOg$3MF9@i@IQg5mjD$pKrs<;#O#1Z(E{@RP`rn61E;>u?t$LoqqH0 zLi(_u#^p`!Y(#3hZUkD^`0aWG7y-e6|I$r>4mWyYp%9_JdL)wuCtj|M?Xeph8 z+Z0%Upf-3cNCrY#BUfDh07IpWk*KjuK^vORY4>o+l&Zr)$*{>@Jk0bGURGQgDQMFz z()s4P;v@rU5PLGVMk?`n_DQ+-N`KKEO!iy{1%jP=zY%-=onzMcUD1*_04;P9=Zk+t zRm?0;$1cnxhFo1+06Ml*WmrDKfbyMf{tGJcm-%3oGN>?ljRlW8y)gmD5}M$rJghlG za8sNTzrJj)B@?H)bc-5Ji94becPA{<#s}9#DvA02C0^AwDq# z$_|QEIqbDj8YYrTtDZJKLNdf^kzyW}IMx>t&s(`L3$Xt%bMj^_ZI>V12hN{n>pO9R zE1;GldbFdrY~HSd_jxIM>FI*gvlqrRR_C>29Ig4^&dcJib*fI#M)q?0xqb9$5}SR( z1{iJlOb~RiHB3jCg=L*>{AxHzxmTg9V1GgbQ}i}bU&EKgc+1! z$TwV`NYpB4{dsR^H8nhN&8wKj;43OBI!yX9qGJQh*gh6>W~7+R=`>0DcoD(L+6o{S zNeT>TH%GRpI%mingyq>S+g%Jd$L(}Nq3g_3c3F>p^Y}&{9)j_sbSYg0Zjay8*Is3= z^AP95DK(U@L~(-mbQ4j+?sYv@JXW8W^m@(`i*36@E&|!HuB%MQAy_LMT`Q2RZT)nG zkTZ&;#s)&2JBLv_5wzYcpbGun-G(c$#>>2elIIrkaopYlDV)Krz4W~$xmTvUsA(oS zb+h!jk)7SCpeQ4Ypl)aIa5|y&&D}oT^w#-DlXKaED&RvcQNwmP?{~PVygO&vR_fs7 zBI_vfWD6^~Lg$UqIirJy`98MOyfk;aS&Mc31s!n;UqdCW(XJNgHALQnRCCP{pO={t zFQb3eLTHh&NLM~*HaT&wK>Q@PLG1Etbm1e#xI?_zo|?z#OylL+(tsx#z$0IN1qOM! zaYz#i`Qznm@(Z;-r?P6Hk4j8nrLmdmG|a;^dF+Q5M*nf5EVzUvil5KVSeQwpXMU&$ zI`HR`thBmAKkw|PNb1(UsyIY=w6KrKSNhbI#tQ)c7%x!5+>jjn2bN=*d1^HSgdlgVxBM~P%8~AE}Fa>m#3vlV+H)j@{ zn|TR6N{a!B*vMqzef z=ni|g6yfAJv1pLa=j&?NPrDoB=kF&X0WEgSg`cr)j-P5|K~$z5V?1Gs{-KK2;(2(X5dBg#fsaMSW4Vn)3O zfP;A~bl`7KJaQgc_@!*RWu8zgkd;3O(V({UR@U?xb+sicOn;t*w6z;YGB|mXiv`sj zxL#md8>;WsYt}f$vyqTvvVqp3_S-AAf6$+yc8GD^glD*56B4;9Dx}7nu*;PVe8){| zG|&!nkN2&h2tj8)AbtEmQ%eE=u8qOO+ImPdaBT|m#{foaBjHZCAa(?X@}qoNtasz_$$K(}DvtUhm5sbt*}G+j^k zNbXT3v|nOr_F&jgSow#37pSp}nTN?E2Zy_lmrX2ix`DFpfH$mozH8b+v6D;j118q& z$-$p%cUWI`Cg&DXpM<$L-%pj-CD*i#C8*b;c!rRYcq#M!GXx7m?(rQl=5!?~nWo`4 zTw(Rw8Zxwx`31Sfx#J(CJEg6Q6teZO*N38cqEBm5u!DxA;aO3Xyu&r77|a-#KU< zm?LL)!hOpmd{@_XyYi*)GKQE5Iht#g^^+Zqf>ju0plQ3a>;O2$CFdG&VG_c8$6;j&omPAI^VE2Gr>!L)HXFP$XYD- zc&aR7$yRVpkLb1hFr@5u7lr}9j!oh5tc9W15+61whoLPGV{(5B|+u?XR(%^%KwDRreZHA}-#41i0yXfI!wwU>;HS@4>_3$4; zycgYk4C2%~)MDhlVPZu%@VY}0RUEdL#+99-3hHwMze4@f59dGD{}}4;9{z7B^X~}#MdUC1KMDP3 zKkL5}`uz`$_3z^Tmn8c)+Uj53t^ZErL;XMV@>hNoWnh5+OdixfCKv($2qF0A?SBDx CZ&8N; diff --git a/powerboardcode/powerboard/powerboard.h b/powerboardcode/powerboard/powerboard.h deleted file mode 100644 index 20c1d29..0000000 --- a/powerboardcode/powerboard/powerboard.h +++ /dev/null @@ -1,67 +0,0 @@ - -#include "Energia.h" -#include "RoveComm.h" -//12V Current Sensing -#define MULTIMEDIA_SENSE PD4 -#define NAV_SENSE PD0 -#define GIMBAL_ACT_SENSE PD1 -#define DRIVE_SENSE PK3 -#define SCISENSOR_ACT_SENSE PD2 -#define NETSWITCH_SENSE PB5 -#define CAM1_SENSE PB4 -#define CAM2_SENSE PK0 -#define BBB_SENSE PD7 -#define AUX_LOG_SENSE PK2 - -//12V CTL -#define MULTIMEDIA_CTL PL1 -#define NAV_CTL PN4 -#define GIMBAL_ACT_CTL PA4 -#define GIMBAL_LOG_CTL PG0 -#define DRIVE_CTL PL2 -#define SCISENSOR_ACT_CTL PL3 -#define SCISENSOR_LOG_CTL PF3 -#define NETSWITCH_CTL PL0 -#define CAM1_CTL PF1 -#define CAM2_CTL PF2 -#define BBB_CTL PN5 -#define AUX_LOG_CTL PB2 -#define SPARE_CTL PL5 - -//PACK CURRENT -#define P_MOTOR1_SENSE PE4 -#define P_MOTOR2_SENSE PE5 -#define P_MOTOR3_SENSE PD3 -#define P_MOTOR4_SENSE PE0 -#define P_MOTOR5_SENSE PE1 -#define P_MOTOR6_SENSE PE2 -#define P_MOTOR7_SENSE PE3 -#define P_POE_SENSE PD5 -#define P_AUX_SENSE PK1 - - -//PACK BUSSES -#define P_MOTOR1_CTL PC4 -#define P_MOTOR2_CTL PC5 -#define P_MOTOR3_CTL PC6 -#define P_MOTOR4_CTL PC7 -#define P_MOTOR5_CTL PP0 -#define P_MOTOR6_CTL PP1 -#define P_MOTOR7_CTL PQ0 -#define P_POE_CTL PP4 -#define P_AUX_CTL PL4 - -#define MOTOR_DELAY 5000 -#define DRIVE_DELAY 5000 - -uint8_t currentSense12V[] = {MULTIMEDIA_SENSE, NAV_SENSE, GIMBAL_ACT_SENSE, DRIVE_SENSE, SCISENSOR_ACT_SENSE, NETSWITCH_SENSE, CAM1_SENSE, CAM2_SENSE, BBB_SENSE, AUX_LOG_SENSE}; -uint8_t actuation12V[5] = {GIMBAL_ACT_CTL, AUX_LOG_CTL, SPARE_CTL, MULTIMEDIA_CTL, SCISENSOR_ACT_CTL}; -uint8_t logic12V[8] = {GIMBAL_LOG_CTL, DRIVE_CTL, NETSWITCH_CTL, NAV_CTL, CAM1_CTL, CAM2_CTL, BBB_CTL, SCISENSOR_LOG_CTL}; -//uint8_t drivePack = P_DRIVE_CTL; -uint8_t currentSensePack[9] = {P_MOTOR1_SENSE, P_MOTOR2_SENSE, P_MOTOR3_SENSE, P_MOTOR4_SENSE, P_MOTOR5_SENSE, P_MOTOR6_SENSE, P_MOTOR7_SENSE, P_POE_SENSE, P_AUX_SENSE}; -uint8_t bussesPack[4] = {P_POE_CTL, , P_AUX_CTL, }; -uint8_t bussesMotor[7] = {P_MOTOR1_CTL, P_MOTOR2_CTL, P_MOTOR3_CTL, P_MOTOR4_CTL, P_MOTOR5_CTL, P_MOTOR6_CTL, P_MOTOR7_CTL}; - -RoveCommEthernet RoveComm; -rovecomm_packet packet; -EthernetServer TCPServer(RC_ROVECOMM_POWERBOARD_PORT); diff --git a/powerboardcode/powerboard/powerboard.ino b/powerboardcode/powerboard/powerboard.ino deleted file mode 100644 index 6b04020..0000000 --- a/powerboardcode/powerboard/powerboard.ino +++ /dev/null @@ -1,216 +0,0 @@ -#include "powerboard.h" -void setup() -{ - Serial.begin(9600); - setPins(); - setPinStates(); - Serial.begin(9600); - RoveComm.begin(RC_POWERBOARD_FOURTHOCTET, &TCPServer); - delay(100); -} - -void loop() -{ - packet = RoveComm.read(); - if(packet.data_id != 0) - { - switch(packet.data_id) - { - case RC_POWERBOARD_MOTORBUSENABLE_DATA_ID: - Serial.println("Enable/Disable Motor Busses"); - for(int i = 0; i < 7; i++) //way i<7 - { - if (packet.data[0] & 1< Date: Fri, 25 Feb 2022 16:50:11 -0600 Subject: [PATCH 06/14] Revert "restructured folders and naming" This reverts commit e87c89e980fecbb4446e2c839ee2c89069c7588a. --- PowerBoard_Software/PowerBoard_Software.h | 154 ------- PowerBoard_Software/PowerBoard_Software.ino | 445 -------------------- PowerBoard_Software_Rev2.zip | Bin 0 -> 7712 bytes powerboardcode/powerboard/powerboard.h | 67 +++ powerboardcode/powerboard/powerboard.ino | 216 ++++++++++ 5 files changed, 283 insertions(+), 599 deletions(-) delete mode 100644 PowerBoard_Software/PowerBoard_Software.h delete mode 100644 PowerBoard_Software/PowerBoard_Software.ino create mode 100644 PowerBoard_Software_Rev2.zip create mode 100644 powerboardcode/powerboard/powerboard.h create mode 100644 powerboardcode/powerboard/powerboard.ino diff --git a/PowerBoard_Software/PowerBoard_Software.h b/PowerBoard_Software/PowerBoard_Software.h deleted file mode 100644 index 10a8be9..0000000 --- a/PowerBoard_Software/PowerBoard_Software.h +++ /dev/null @@ -1,154 +0,0 @@ -//Program: PowerBoard_Software_Functions.h -//Programmer: Evan Hite -//Purpose: To hold all constants and function prototypes for -// PowerBoard_Software.ino and PowerBoard_Software_Functions.cpp - -#ifndef POWERBOARD_SOFTWARE_FUNCTIONS_HEADER_H -#define POWERBOARD_SOFTWARE_FUNCTIONS_HEADER_H -#include "RoveComm.h" -#include -using namespace std ; - -RoveCommEthernetUdp RoveComm; - -//Struct For Motor Busses, Below are Global Constants -struct PB_Bus -{ - uint8_t rovecomm_cell ; //Bit position in RoveComm packet, either 0 for ACL, or 1 for Motor Busses - uint8_t bit_code_off ; //Binary code with 0 being the position in which the bus exists in the rovecomm packet cell. - uint8_t bit_code_on ; //Binary code with 1 being the position in which the bus exists in the rovecomm packet cell. - int imeas_pin ; //current measurment pin on the board - int ctl_pin ; //control pin on the tiva - int amp_threshold ; //Amp Threshold on the bus - int bus_tuning ; //Tuning for each bus current reading - String identity ; //Identity of the bus -//Basically a constructor, but I do not need it for that purpose -//I am simply wanting to be able to adjust all the values at once -void Adjust_Values (const uint8_t & Rovecomm_cell, const uint8_t & Bit_code_off, const uint8_t & Bit_code_on, const int & Imeas_pin, const int & Ctl_pin, const int & Amp_threshold, const int & Bus_tuning, const String Identity) ; -} ; - -//Global Constants - -//Delays -#define ROVER_POWER_RESET_DELAY 3000 //Delay to reset rover power -#define ROVECOMM_DELAY 10 //Delay to send Rovecomm a package -#define DEBOUNCE_DELAY 10 //Delay after current or voltage trip -#define ROVECOMM_UPDATE_DELAY 1000 //Delay so that Rovecomm is not overloaded -#define COMM_OFF_DELAY 5000 //Delay so that comms turn on after being turned off -#define MOTOR_DELAY 5000 //Delay so that the motors turn on after everything else -#define ROCKET_OFF_DELAY 5000 //Delay so that rockets turn on after being turned off - -//////////////////////////////////////////////Pinmap -// Control Pins for Busses -#define ACT_CTL_PIN PN_3 //In rev 2 -#define LOGIC_CTL_PIN PD_1 //In rev 2 -#define COMM_CTL_PIN PH_2 //In rev 2 -#define COMM_LOGIC_CTL_PIN PP_2 //In rev 2 -#define EM_CTL_PIN PK_5 //Was Auxillary -#define FM_CTL_PIN PK_7 //Was M1 -#define MM_CTL_PIN PK_6 //Was M2 -#define BM_CTL_PIN PH_1 //Was M3 -#define AUX_CTL_PIN PH_0 //Was M4 -#define ROCKET_CTL_PIN PM_2 //Was M7 -#define GE_CTL_PIN PM_1 //Was M6 -//#define M7_CTL_PIN PM_0 //No longer in use -#define FAN_CTL_PIN PM_3 //In rev 2 - -// Sensor Volts/Amps Readings Pins -#define ACT_I_MEAS_PIN PE_2 //In rev 2 -#define LOGIC_I_MEAS_PIN PE_1 //In rev 2 -#define COMM_I_MEAS_PIN PE_0 //In rev 2 -#define EM_I_MEAS_PIN PD_0 //Was Auxillary in rev 1 -#define FM_I_MEAS_PIN PK_3 //was M1 -#define MM_I_MEAS_PIN PK_2 //was M2 -#define BM_I_MEAS_PIN PK_1 //was M3 -#define AUX_I_MEAS_PIN PK_0 //was M4 -#define ROCKET_I_MEAS_PIN PB_5 //Was M5 -#define GE_I_MEAS_PIN PB_4 //Was M6 -//#define M7_I_MEAS_PIN PE_3 //No longer in use -//#define PACK_VOLTAGE_PIN PE_5 //No longer in use - -//////////////////////////////////////////////RoveBoard -// Tiva1294C RoveBoard Specs -#define VCC 3300 //volts -#define ADC_MAX 4096 //bits -#define ADC_MIN 420 //meme bits - -//////////////////////////////////////////////Sensor -// ACS722LLCTR-40AU-T IC Sensor Specs -#define SENSOR_SENSITIVITY 0.066 //volts/amp -#define SENSOR_SCALE 0.1 //volts/amp -#define SENSOR_BIAS VCC * SENSOR_SCALE - -#define CURRENT_MAX 40000//((VCC - SENSOR_BIAS) / SENSOR_SENSITIVITY) -#define CURRENT_MIN 0//-SENSOR_BIAS / SENSOR_SENSITIVITY - -#define VOLTS_MIN 0 -#define VOLTS_MAX 33600 - -//Safest Test pin -#define ESTOP_12V_COMM_LOGIC_MAX_AMPS_THRESHOLD 5000 //5 amps, 5 amp fuse -#define ESTOP_12V_ACT_MAX_AMPS_THRESHOLD 15000 //15 amps, 20 amp fuse -#define ESTOP_AUX_MAX_AMPS_THRESHOLD 21000 //19 amps, 20 amp fuse(360W/21.6) = 16.6 Amps, due to inaccurate readings raised to 19 -#define ESTOP_MOTOR_BUS_MAX_AMPS_THRESHOLD 30000 //30 amps, 40 amp fuse -#define ESTOP_ROCKET_BUS_MAX_AMPS_THRESHOLD 4000 //4 amps, 5 amp fuse -#define ESTOP_GEN_EXTRA_BUS_MAX_AMPS_THRESHOLD 30000 // 30 Amps, 40 Amp fuse - -//Tuning Variables -#define LOGIC_COMM_TUNER 1010 //Tuner for ADC values for Logic and Comm busses -#define ACT_TUNER 1000 //Tuner for ADC values for Actuation bus -#define AUX_TUNER 1020 //Tuner for ADC values for Auxilliary bus -#define MOTOR_TUNER 1005 //Tuner for ADC values for motor busses -#define ROCKET_TUNER 1005 //Tuner for ADC values for Rocket antennas -#define CURRENT_AVERAGE 10 //The amount of current readings that we average over to get accurate current readings - -//Number of Busses -#define ALC_BUSSES 3 //Number of Actuation, logic and Communication busses -#define MOTOR_BUSSES 7 //Number of busses besides actuation, logic, and communication - -//Functions///////////////////////////////////////////////////////////////////////// - -//Description: Checks the pin for bouncing voltages to avoid false positive -//Pre: Bouncing_pin is a pin on the tiva that reads amperage, max_amps_threshold -// is the maximum aperage the device can tolerate -//Post: Returns a bool of true if the pin has too much current -bool singleDebounce(const int & bouncing_pin,const int & max_amps_threshold, const int & Tuner); - -//Description: Sets the pins on the tiva to certain buses on the powerboard -//Pre: None -//Post: All pins on the tiva are set to correct values. -void Configure_Pins (); - -//Description: Turns all pins to on after setting all to low -//Pre: Pins must be in correct position from Configure_Pins -//Post: All pins are set high -void Pin_Initialization (); - -//Description: Begins communication with RoveComm and sends a packet to rovecomm -//Pre: Bus[] must have a size of RC_POWERBOARD_BUSENABLED_DATACOUNT to work. -//Post: Sends to RoveComm a packet that the powerboard is live. -void Communication_Begin (uint8_t Bus []) ; - -//Description: Checks overcurrent on the Bus and shuts off in need be -//Pre: BUS_I_MEAS_PIN must be a pin for current measurement, Bus[] must be the same array from Commuincation_Begin. -// BUS_CTL_PIN must be the relevent pin that controls the bus. and ESTOP_AMP_THRESHOLD must also be the relevent estop threshold. -//Post: Shuts off the bus if there is an overcurrent situation and writes this to Bus[] -bool Shut_Off(const PB_Bus & Bus, uint8_t Send_Recieve[], const uint16_t & Current_Reading) ; - -//Description: Takes a packet from RoveComm and turn on or off a bus -//Pre: Enable_Disable should be a packet recieved from RoveComm, and Bus should be from Shut_Off -//Post: Turns on or off all buses that the packet says should be on or off and writes this to Bus[]. -void Bus_Enable (const rovecomm_packet & Enable_Disable, uint8_t Send_Recieve[], const PB_Bus Bus[]) ; - -//Description: Reads a current sensing pin and saves this value to current_reading -//Pre: Current_Reading should be an array part, for RoveComm reasons. BUS_I_MEAS_PIN should be a current measuring pin -//that corresponds to the spot on the Current_Reading array. -//Post: Returns a value to current_reading of the current reading value from the BUS_I_MEAS_PIN in mA. -void Pin_Read (uint16_t & current_reading, const int & BUS_I_MEAS_PIN, const int & Tuner) ; - -//Description: -//Pre: -//Post: -void Bus_Setup(PB_Bus Bus[]) ; - -#endif diff --git a/PowerBoard_Software/PowerBoard_Software.ino b/PowerBoard_Software/PowerBoard_Software.ino deleted file mode 100644 index cf52889..0000000 --- a/PowerBoard_Software/PowerBoard_Software.ino +++ /dev/null @@ -1,445 +0,0 @@ -//RoveWare Powerboard ACS_722 Interface -// -// Created for Zenith by: Judah Schad, jrs6w7 -// Altered for Gryphon by: Jacob Lipina, jrlwd5 -// Altered for Valkyrie by: Evan Hite erhtpc -// Using http://www.digikey.com/product-detail/en/allegro-microsystems-llc/ACS722LLCTR-40AU-T/620-1640-1-ND/4948876 -// -// Standard C -#include -#include -#include - -//Local Files -#include "PowerBoard_Software_Functions_Header.h" - -//Bus Control Variables -uint8_t Send_Recieve[] {0,0} ; //Bus to Enable or Disable -PB_Bus Bus[RC_POWERBOARD_IMEASmA_DATACOUNT] ; //An array of all the Busses on the Powerboard - -//Packet Reception and Sent Variables -rovecomm_packet Enable_Disable ; //packet reception variable -uint16_t Current_Reading[RC_POWERBOARD_IMEASmA_DATACOUNT] ; //Current Reading for all busses -uint16_t error_reading[CURRENT_AVERAGE][RC_POWERBOARD_IMEASmA_DATACOUNT] ; -bool Bus_Tripped ; //To determine whether or not to send a packet based on overcurrents -bool sent_packet = true ; //To determine whether or not to send a packet of current values -uint32_t last_time_packet = 0 ; //Time since last packet or current values sent -bool Overcurrent = false ; //Shows whether or not a bus has overcurrented -int times_through = 0 ; -int average_holder = 0 ; -uint32_t comm_off_timer = 0 ; -bool comm_off = false ; -uint32_t Motor_Turn_On_Time = 0 ; -bool rockets_off = false ; -uint32_t rocket_off_timer = 0 ; - -//////////////////////////////////////////////Powerboard Begin -// the setup routine runs once when you press reset -void setup() -{ - Serial.begin(115200) ; - delay(500) ; -//Serial.println("Got here...") ; - Configure_Pins () ; //Configures pins to correct busses - Pin_Initialization () ; //Sets pins to low then to high - Bus_Setup(Bus) ; //Sets up the PowerBoard Busses in software - Communication_Begin (Send_Recieve) ; //Sends to base station that everything is now on and communication begins - Motor_Turn_On_Time = millis()+MOTOR_DELAY ; -}//end setup - -/////////////////////////////////////////////Powerboard Loop Forever -void loop() -{ - //Current Readings to Report back to Base Station////////////////////////////////////////////////////////////////////////////////////////// - /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - //Code to setup sending a current reading packet every second - static bool first_run = true; - if(millis() >= Motor_Turn_On_Time && first_run) - { - Serial.println("Motor On"); - first_run = false; - digitalWrite(EM_CTL_PIN, HIGH) ; - digitalWrite(FM_CTL_PIN, HIGH) ; - digitalWrite(MM_CTL_PIN, HIGH) ; - digitalWrite(BM_CTL_PIN, HIGH) ; - } - if(sent_packet == true) - { - last_time_packet = millis() ; //Timestamp - sent_packet = false ; //So last_time_packet will not be overwritten too quickly - } - for(int i = 0 ; i < (RC_POWERBOARD_IMEASmA_DATACOUNT) ; i++) - { - Pin_Read(Current_Reading[i], Bus[i].imeas_pin, Bus[i].bus_tuning) ; - //Serial.print(Bus[i].identity) ; - //Serial.println(" Current Reading") ; - //delay(10) ; - } - for(int k = 0 ; k <(RC_POWERBOARD_IMEASmA_DATACOUNT) ; k++) - { - error_reading[times_through][k] = Current_Reading[k] ; - } - times_through++ ; - //Serial.println("") ; - //End of Current Reads - //Sends Current Values back to basestation every second, after the board has run through the code once - if(millis() >= (last_time_packet+ROVECOMM_UPDATE_DELAY)) - { - RoveComm.write(RC_POWERBOARD_IMEASmA_DATAID, RC_POWERBOARD_IMEASmA_DATACOUNT, Current_Reading) ; //Sends back current readings - delay(ROVECOMM_DELAY) ; - RoveComm.write(RC_POWERBOARD_BUSENABLED_DATAID, RC_POWERBOARD_BUSENABLED_DATACOUNT, Send_Recieve) ; //Sends back present state of motor busses - sent_packet = true ; //So we can update - delay(ROVECOMM_DELAY) ; -//Serial.println("Hello") ; //Debug Code -//delay(10) ; - } - - //Checking for Over Currents on all busses////////////////////////////////////////////////////////////////////////////////////////////////// - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -//Serial.println("Checking comms") ; //Serial Debugging Code -//delay(10) ; - if(times_through >= CURRENT_AVERAGE) - { - times_through = 0 ; //Reset the averaging of the current reads - for(int L = 0 ; L < (RC_POWERBOARD_IMEASmA_DATACOUNT) ; L++) //Averaging the Current Readings over the amount specified by the Current_Average - { - for(int M = 0 ; M < CURRENT_AVERAGE ; M++) - { - average_holder = average_holder + error_reading[M][L] ; - } - average_holder = average_holder/CURRENT_AVERAGE ; - Current_Reading[L] = average_holder ; - average_holder = 0 ; - //Serial.print(Bus[L].identity) ; - //Serial.print(" current reading of ") ; - //Serial.print(Current_Reading[L]) ; - //Serial.println(" mA") ; - } - for(int j = 0 ; j < (RC_POWERBOARD_IMEASmA_DATACOUNT) ; j++) - { - Bus_Tripped = Shut_Off(Bus[j], Send_Recieve, Current_Reading[j]) ; -// Serial.print(Current_Reading[j]) ; -// Serial.print(" ") ; -// Serial.print(Bus[j].identity) ; -// Serial.println(" Overcurrent check") ; -// delay(10) ; - if(Bus_Tripped == true) - { - Overcurrent = true ; - Serial.print(Bus[j].identity) ; - Serial.print(" Overcurrented at") ; - Serial.print(Current_Reading[j]) ; - Serial.println("mA") ; - } - } - //Serial.println("") ; - //End of Overcurrents - //If any bus senses an overcurrent, then a packet will be sent - if(Overcurrent == true) - { - RoveComm.write(RC_POWERBOARD_BUSENABLED_DATAID, RC_POWERBOARD_BUSENABLED_DATACOUNT, Send_Recieve) ; //Send out a summary of what is off after current check - delay(ROVECOMM_DELAY) ; - Bus_Tripped = false ; - Overcurrent = false ; - } - } - /////////////////////////////////////////////RED Control and Telem RoveComm - //Recieves Pack form rovecomm and shuts off or turns on busses at need - Enable_Disable = RoveComm.read(); - if(Enable_Disable.data_id == RC_POWERBOARD_BUSENABLE_DATAID) - { - Bus_Enable(Enable_Disable, Send_Recieve, Bus) ; - //Serial.println("") ; - //Serial.println("Packet Recieved") ; - //Serial.println("") ; - delay(500) ; //Delay half a second after recieving a packet to let the rover turn on and off certain busses. - if(digitalRead(COMM_CTL_PIN) == LOW) - { - comm_off = true ; - comm_off_timer = millis() ; - } - if(digitalRead(ROCKET_CTL_PIN) == LOW) - { - rockets_off = true ; - rocket_off_timer = millis() ; - } - } - if(comm_off == true) - { - if(millis() >= (comm_off_timer+COMM_OFF_DELAY)) - { - digitalWrite(COMM_CTL_PIN, HIGH) ; - comm_off = false ; - delay(1000) ; //Delay a second to let everything turn on - } - } - if(rockets_off == true) - { - if(millis() >= (rocket_off_timer+ROCKET_OFF_DELAY)) - { - digitalWrite(ROCKET_CTL_PIN, HIGH) ; - rockets_off = false ; - delay(1000) ; //Delay a second to let it turn on - } - } -} -//End of Main Loop - - -///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -//Functions -///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - -//Function 1.//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool singleDebounce(const int & bouncing_pin, const int & max_amps_threshold, const int & Tuner) -{ - int adc_threshhold = ((map(max_amps_threshold, CURRENT_MIN, CURRENT_MAX, ADC_MIN, ADC_MAX)*1000)/(Tuner)); //Get reading off pin - bool trip = false ; - if(analogRead(bouncing_pin) > adc_threshhold) //If pin reading is high - { - delay(DEBOUNCE_DELAY); - if( analogRead(bouncing_pin) > adc_threshhold) //If pin reading is still high - { - trip = true; //Sends back true to indicate shut off - }//end if - else - { - trip = false ; - } - }// end if - return trip; -} - -//Function 2.//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void Configure_Pins () -{ - // Control Pins are outputs - pinMode(ACT_CTL_PIN, OUTPUT); - pinMode(COMM_LOGIC_CTL_PIN, OUTPUT); - pinMode(COMM_CTL_PIN, OUTPUT); - pinMode(LOGIC_CTL_PIN, OUTPUT); - pinMode(EM_CTL_PIN, OUTPUT); - pinMode(FM_CTL_PIN, OUTPUT); - pinMode(MM_CTL_PIN, OUTPUT); - pinMode(BM_CTL_PIN, OUTPUT); - pinMode(AUX_CTL_PIN, OUTPUT); - pinMode(ROCKET_CTL_PIN, OUTPUT); - pinMode(GE_CTL_PIN, OUTPUT); - pinMode(FAN_CTL_PIN, OUTPUT); - - //Current Measurement pins are inputs - pinMode(ACT_I_MEAS_PIN, INPUT); - pinMode(COMM_I_MEAS_PIN, INPUT); - pinMode(LOGIC_I_MEAS_PIN, INPUT); - pinMode(EM_I_MEAS_PIN, INPUT); - pinMode(FM_I_MEAS_PIN, INPUT); - pinMode(MM_I_MEAS_PIN, INPUT); - pinMode(BM_I_MEAS_PIN, INPUT); - pinMode(AUX_I_MEAS_PIN, INPUT); - pinMode(ROCKET_I_MEAS_PIN, INPUT); - pinMode(GE_I_MEAS_PIN, INPUT); - return ; -} - -//Function 4.//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void Pin_Initialization () -{ - digitalWrite(ACT_CTL_PIN, LOW); - digitalWrite(COMM_LOGIC_CTL_PIN, LOW); - digitalWrite(COMM_CTL_PIN, LOW); - digitalWrite(LOGIC_CTL_PIN, LOW); - digitalWrite(EM_CTL_PIN, LOW); - digitalWrite(FM_CTL_PIN, LOW); - digitalWrite(MM_CTL_PIN, LOW); - digitalWrite(BM_CTL_PIN, LOW); - digitalWrite(AUX_CTL_PIN, LOW); - digitalWrite(ROCKET_CTL_PIN, LOW); - digitalWrite(GE_CTL_PIN, LOW); - digitalWrite(FAN_CTL_PIN, LOW); - // Turn on everything when we begin - delay(ROVER_POWER_RESET_DELAY); //Three Second Delay - - //After Delay, make sure everything turns on - digitalWrite(ACT_CTL_PIN, HIGH); - digitalWrite(COMM_LOGIC_CTL_PIN, HIGH); - digitalWrite(COMM_CTL_PIN, HIGH); - digitalWrite(LOGIC_CTL_PIN, HIGH); - //digitalWrite(EM_CTL_PIN, HIGH); - //digitalWrite(FM_CTL_PIN, HIGH); - //digitalWrite(MM_CTL_PIN, HIGH); - //digitalWrite(BM_CTL_PIN, HIGH); - digitalWrite(AUX_CTL_PIN, HIGH); - digitalWrite(ROCKET_CTL_PIN, HIGH); - digitalWrite(GE_CTL_PIN, HIGH); - digitalWrite(FAN_CTL_PIN, HIGH); //Fans Always On - return ; -} - -//Function 5.///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void Communication_Begin (uint8_t Send_Recieve []) //This function may have to change -{ - RoveComm.begin(RC_POWERBOARD_FOURTHOCTET); - delay(100) ; - Serial.println("Setting Up..."); - delay(ROVECOMM_DELAY); - uint8_t ALC_Bits = 1 ; - uint8_t Motor_Bits = 1 ; - uint8_t binary_hold = 2 ; - for(int i = 1 ; i < ALC_BUSSES ; i++) - { - ALC_Bits = ALC_Bits + binary_hold ; - binary_hold = 2*binary_hold ; - } - binary_hold = 2 ; - for(int i = 1 ; i < MOTOR_BUSSES ; i++) - { - Motor_Bits = Motor_Bits + binary_hold ; - binary_hold = 2*binary_hold ; - } - Send_Recieve[0] = ALC_Bits ; - Send_Recieve[1] = Motor_Bits ; - RoveComm.write(RC_POWERBOARD_BUSENABLED_DATAID, RC_POWERBOARD_BUSENABLED_DATACOUNT, Send_Recieve) ; //Initial states of Busses -} - -//Function 6.//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool Shut_Off( const PB_Bus & Bus, uint8_t Send_Recieve[], const uint16_t & Current_Reading) -{ - bool Bus_Tripped = false ; - //Special Variables in case of the communication bus being tripped - static uint32_t time1 = 0 ; - static bool comms_off = false ; - const uint8_t comm_on_bit_code = 4 ; - if(Current_Reading > Bus.amp_threshold ) //If pin is tripped - { - Bus_Tripped = true ; - //Special Case for the Communication Bus - if(Bus.ctl_pin == COMM_CTL_PIN)//Special rules for communication bus - { - Send_Recieve[0] = Send_Recieve[0] & Bus.bit_code_off ; - RoveComm.write(RC_POWERBOARD_BUSENABLED_DATAID, RC_POWERBOARD_BUSENABLED_DATACOUNT, Send_Recieve) ; //Send a packet to rovecomm prior to shutting off comms - delay(ROVECOMM_DELAY) ; - digitalWrite(COMM_CTL_PIN, LOW); //Turn off the comms - if(time1 == 0) //Timing Code, to turn on comms in 10 seconds, first half - { - comms_off = true ; - time1 = millis(); - } - } - else //All other busses - { - Send_Recieve[Bus.rovecomm_cell] = Send_Recieve[Bus.rovecomm_cell] & Bus.bit_code_off ; - //Bitwise and with ones in bit code will result in only changing the specific bit I want changed - digitalWrite(Bus.ctl_pin, LOW) ; //Turning off the bus - delay(ROVECOMM_DELAY) ; //Just adding a delay in case the switch takes longer than expected - } - } - if(comms_off == true) //Second half of timing code to turn on comms after 10 seconds - { - if(millis()>=(time1+COMM_OFF_DELAY)) //it is turned off in case the overcurrent was just a random spike. - { - comms_off = false ; //If there actually is a short in the bus, the bus will turn itself - time1 = 0 ; //back on - digitalWrite(COMM_CTL_PIN,HIGH); - Send_Recieve[0] = Send_Recieve[0] | comm_on_bit_code ; - } - } - return Bus_Tripped ; -} - -//Function 7.////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void Bus_Enable (const rovecomm_packet & Enable_Disable, uint8_t Send_Recieve[], const PB_Bus Bus[]) -{ - int ALC_to_Motor_Count = ALC_BUSSES ; - int Start_Point = 0 ; - int bit_helper = 1 ; - Serial.println("Bus En"); - Serial.println(Enable_Disable.data[0]); - Serial.println(Enable_Disable.data[1]); - Serial.println(Enable_Disable.data[2]); -for(int i = 0 ; i < (RC_POWERBOARD_BUSENABLE_DATACOUNT) ; i++) -{ - for(int j = Start_Point ; j < ALC_to_Motor_Count ; j++) - { - //Serial.println(j) ; - //delay(10) ; - //Serial.print(Enable_Disable.data[i]) ; - //Serial.println( " <- data sent") ; - //delay(10) ; - if(bitRead(Enable_Disable.data[i] , (bit_helper)) == 1) //Changing State of the Bus - { - //Serial.print("Changing State of ") ; - //delay(10) ; - if(Enable_Disable.data[2] == 1) //Turn on - { - Serial.print(Bus[j].identity) ; - Serial.println(" Turned on by user") ; - //delay(10) ; - Send_Recieve[i] = Send_Recieve[i] | Bus[j].bit_code_on ; - digitalWrite(Bus[j].ctl_pin, HIGH) ; - delay(ROVECOMM_DELAY) ; - } - else //Turn OFF - { - Serial.print(Bus[j].identity) ; - Serial.println( " Turned off by user") ; - //delay(10) ; - Send_Recieve[i] = Send_Recieve[i] & Bus[j].bit_code_off ; - digitalWrite(Bus[j].ctl_pin, LOW) ; - delay(ROVECOMM_DELAY) ; - } - } - bit_helper++ ; - } - Start_Point = ALC_to_Motor_Count; - ALC_to_Motor_Count = ALC_to_Motor_Count + MOTOR_BUSSES ; - bit_helper = 1 ; -} - return ; -} - -//Function 8.//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void Pin_Read (uint16_t & current_reading, const int & BUS_I_MEAS_PIN, const int & Tuner) -{ - // Using http://www.digikey.com/product-detail/en/allegro-microsystems-llc/ACS722LLCTR-40AU-T/620-1640-1-ND/4948876 - int adc_reading = analogRead(BUS_I_MEAS_PIN) ; - if(adc_reading < ADC_MIN) - { - adc_reading = ADC_MIN ; - } - if(adc_reading > ADC_MAX) - { - adc_reading = ADC_MAX ; - } - current_reading = ((((map(adc_reading, ADC_MIN, ADC_MAX, CURRENT_MIN, CURRENT_MAX)*1180)/1000)*Tuner)/1000); - //Serial.println(current_reading) ; - return ; -} - -void PB_Bus::Adjust_Values (const uint8_t & Rovecomm_cell, const uint8_t & Bit_code_off, const uint8_t & Bit_code_on, const int & Imeas_pin, const int & Ctl_pin, const int & Amp_threshold, const int & Bus_tuning, const String Identity) -{ - rovecomm_cell = Rovecomm_cell ; - bit_code_off = Bit_code_off ; //Binary converted to decimal (255-bit_code_on) - bit_code_on = Bit_code_on ; //Binary converted to decimal, 2^(place in cell) - imeas_pin = Imeas_pin ; - ctl_pin = Ctl_pin ; - amp_threshold = Amp_threshold ; - bus_tuning = Bus_tuning ; - identity = Identity ; -} -void Bus_Setup(PB_Bus Bus[]) -{ - Bus[0].Adjust_Values(RC_POWERBOARD_BUSENABLE_ALCENTRY, 254, 1, ACT_I_MEAS_PIN, ACT_CTL_PIN, ESTOP_12V_ACT_MAX_AMPS_THRESHOLD, ACT_TUNER, "Actuation") ; - Bus[1].Adjust_Values(RC_POWERBOARD_BUSENABLE_ALCENTRY, 253, 2, LOGIC_I_MEAS_PIN, LOGIC_CTL_PIN, ESTOP_12V_COMM_LOGIC_MAX_AMPS_THRESHOLD, LOGIC_COMM_TUNER, "Logic") ; - Bus[2].Adjust_Values(RC_POWERBOARD_BUSENABLE_ALCENTRY, 251, 4, COMM_I_MEAS_PIN, COMM_CTL_PIN, ESTOP_12V_COMM_LOGIC_MAX_AMPS_THRESHOLD, LOGIC_COMM_TUNER, "Communications") ; - Bus[3].Adjust_Values(RC_POWERBOARD_BUSENABLE_MOTORSENTRY, 254, 1, FM_I_MEAS_PIN, FM_CTL_PIN, ESTOP_MOTOR_BUS_MAX_AMPS_THRESHOLD, MOTOR_TUNER, "Front Motors") ; - Bus[4].Adjust_Values(RC_POWERBOARD_BUSENABLE_MOTORSENTRY, 253, 2, MM_I_MEAS_PIN, MM_CTL_PIN, ESTOP_MOTOR_BUS_MAX_AMPS_THRESHOLD, MOTOR_TUNER, "Middle Motors") ; - Bus[5].Adjust_Values(RC_POWERBOARD_BUSENABLE_MOTORSENTRY, 251, 4, BM_I_MEAS_PIN, BM_CTL_PIN, ESTOP_MOTOR_BUS_MAX_AMPS_THRESHOLD, MOTOR_TUNER, "Back Motors") ; - Bus[6].Adjust_Values(RC_POWERBOARD_BUSENABLE_MOTORSENTRY, 247, 8, EM_I_MEAS_PIN, EM_CTL_PIN, ESTOP_MOTOR_BUS_MAX_AMPS_THRESHOLD, MOTOR_TUNER, "Extra Motors") ; - Bus[7].Adjust_Values(RC_POWERBOARD_BUSENABLE_MOTORSENTRY, 239, 16, ROCKET_I_MEAS_PIN, ROCKET_CTL_PIN, ESTOP_ROCKET_BUS_MAX_AMPS_THRESHOLD, ROCKET_TUNER, "Rockets") ; - Bus[8].Adjust_Values(RC_POWERBOARD_BUSENABLE_MOTORSENTRY, 223, 32, GE_I_MEAS_PIN, GE_CTL_PIN, ESTOP_GEN_EXTRA_BUS_MAX_AMPS_THRESHOLD, MOTOR_TUNER, "General Extra") ; - Bus[9].Adjust_Values(RC_POWERBOARD_BUSENABLE_MOTORSENTRY, 191, 64, AUX_I_MEAS_PIN, AUX_CTL_PIN, ESTOP_AUX_MAX_AMPS_THRESHOLD, AUX_TUNER, "Auxillary") ; -} - - - diff --git a/PowerBoard_Software_Rev2.zip b/PowerBoard_Software_Rev2.zip new file mode 100644 index 0000000000000000000000000000000000000000..19f3c1e63daa9cf6f467df9fba0e83402f06dd7a GIT binary patch literal 7712 zcmb7pWpEulvgI}1n6H_cV|L8U6f;B2OtBp^(>2D-j4?CC%*@Qp%nY%;GxO%FeY5+$ zty-z1BbC(Mr}|e{A4M4`XbiwVhezX+{J#$W-f;hcigvE14kC7j4kr34c4khlh7P9s z%BC(~m=ynQ2Kz6wjh`-tr0@U$FA)GB`2RHf8-Ehcw#H7DcD9cCQl^F`rVfl26N4Oq~%gq_4(hU3lB8!f-X7(e+0d;hu(p379-d(bd8ZVwV z>kGkT9608T@ET@0%tF)xBNnAe8LudA%1yKZ7)pQ^`}1zUi(>g_Ab^u)k_Cc?4{PWH zX1k|1cm5^GZWn1auK&-kMuLgVeHzViySM0x83fx)^Gd<2N%ii`!-zIh`^6V53-_^i zR_^FtnpNhTab-RhjkCwhbdhh-7?-$@+lyWP;$UA{+vD(>lZOa(0}?8iQR0lsY&*!lN#W5Y4DCePjX%4%Wc(x zHcKHBv9xa4VEcn%j9S^P``Fw`xg@e6kh>NT$03Fi`?Fp7)=H+Gso-A6Olb`XB|$2X$2jErAZu9ol9D7!>kb?CM}+2}7MU%!-k*fhxE7TH>vI_HDoWN)PnTRb zS_@V^04ISg?h*vJM(i#nAnRfctRxSva=JCAQYpu>+NZ`dRm1{_^^sch!*!5Gk--t& z%>|+L4RpJjXd&Nllqi5?w%u(xCa4XOsRjnte;aZ}rh(;`f6SNUI>+Kpo@b~3!AuD9 z4IO#&Fxp)T6~7Z-5&4}VC`75ePpj!cHwlv1=zETg8P%(jMC4Ax3^e5VKzHbB(ek%H zoVA=-uDX)OvMU#dQwtL!PTePR@8!mjxP;|gFRa!ivPVHsAp7c{?AH@=aPt@*yL-s> zR{Nw}%uL(2rQDB8iw@Ufh#y`UbI{lbJb1xD6p@NEgFDDbQcec419K7l4IzpgeLMKT zg#k{{LDk5^LM_QoAx-pg<@wdG-M5_-CS=uHUP@!?k7Zdf(T{h2uVvP>pop$Q>(Qpf znICUm_tf1+eB_DdEC`G>B7G>z3MK})RJV`Dk2s@77Nk>OWt;nY)NY zTe4>{b7{~gp~Qg{nJ6yYN}s`8k4v(Aptjc?G_FxuC0ufw#-ePVeS1Q~yDV*FY>USCT{ zIPWoR?B3x)SCFaDdb#@G8?`?xhetL&x}>wOAp3opEMs?oKZ<1*RJ7XOB($(-x}=fs zDVD6^+?I9@aq0}WMsTiEvs z1;djj4|>9>2L2B(7x%_EHF+Gdc;)p8gsbembVCY?91Ts!AoJKPr{Wud^;Dff;M`Wu z6hRPemhfg*`}ZDQzZc?Gu+QNE=5y+u9S>)p5?i|LI?WatPS59rLi_B|#@IlIVGgcU z-*4y=*-ycbW$|RX5B1E+{N!V5mkGH}a(p2!1vQm;R8{!L?VJd0`fmk}coir}dJ(9V zTa?Kark8;rwr#iFxcLgD!4O};+a$MyklwZ(gRqXbXdOpl6@BWR*pO1xUwFXN0f-sE zKB(!6J`24NxBnZrNFrx0aAJ!fYFUr_tO}E4Ujs=E*2xqOM?=Ql|^t z$5ez^T|0f(kM2+&MX}9?{S(#1i{&GN&sv@{;%f~t8h~{BTBiCPem-nzM(SWapSrLm zcj}q|4>*6Youk{{31*zDj7_;1iTqw$lnq4>DSirQ)UFKdmhPC`0#=sHF9q{oqv*&% zXo)db;#Y|jUT$?oG>Z9sx)#rW;etyL?f6}KPD4vJB!J-R-c*ml(m+ejVf6Kfi;}H= zxlHMA8Fck06>m@?VU>@~l4rlN^>?k`v3@S!;*=``!uR7_Jah4^n%m9E7%^P)iv0^# z@~1u&Qy|%pRN`|8=aqU^uaX4KrUca=x5LQ;URE-IgfV9o1hF--LGfJ3-pe{)f*J;W z7SjAlZ~DXEcR5gBS5A=R%I|pJn`Unyb(y0e0wn%0B@#)R64gp>RUO1y1+Q>09i*OI zOU`4pA&($WeSOF7E!J)ko~9oRd^DS|--(2@IN)Ne5BBQw#2PM& zQnLv-T-dd8!SD}<07&kDAve>01k@(AmR8S6Ez|i_bdG+R*3o6 zH0?pc+MGRQwY*-E$R23+u`RehsZy03`yq)YB{RKF9YD zDn)ONb}Iuss6m~JS%q>~YtyT*YpWY+QVCHQ>|^{F{ce2l5;{B75WOa-5@Pk;%;vfte{!s^ig(B=UPTU5bh-n6 zevltr@cGTxOf!?)@zwOQ!k=(8IlYq2c8Hx0(h7!_h!M~KilGl-v*8`)Kda9uz%$4f z6#%%C1OV9of2+@bR?)3Q-k5&)-E{e@^!)$ zCJ<;ji1^n2B*ML5InY;=b5DMZ+6FK14Ev(>I%1vQr?*3fY9az@kkwq-{IciB?HMh1 zm1rz!>TrA42Pq;-J{=w7;BN%#6ASgtnYdQh$)Uy)(|`0%>yQBYCm;-h?X?TeF` z=w*gISIoQEKm;z#57~YgAgvrOoMTz5O=&7BKhQW~BM9W~OG2dE+jljeIyPkdh|v#S zxd!9RWBx!%>ZQ1Km4}}7b4AhM!U~jxw1d`9ypz`?p!%~r4HltPMyX7ey6EJAzTl_+ zdA^kC*goK|3COB}Jxf8C{YguoLk0;`@|NHx#_HTAHN}$wGg>T_5B_PuNj(Ku~xD$;t4TjuHB&3(}zf4Tr-_efqbt?$3}2#5&g{J4O@yWG+9*mvj)arWUTO*el| zCb%s=Vf5oi4j{{Q6|*^CqQmMgGNM2rFnS>WUoB$?W_Ycw9!5xJegj)7PU2}pFYof_>`|wmX>q~^Q|S7m9*tqSb>~~^ zTYqJFd;d@~w_}>vT+oD@TILW+UIjRR_0I}KaY#=_Q5Y$2zPE6db|pc%^AeuIQ!{jg zlNFN@5=|c?ELt~(OBle=g?#Ybi6U;36eg#FQCri}L2cf?#8+Rh-d{wqwNqdr3m|yW z{GC$N)aV=`f7J*v#~q7w&FJlr+=Z3=1Sr}|1NE!1J3`W|KPib~Rs>=acuRtRDsJ{j z>2Ey=+avmJ+VozD25*auB&=50`Fc0OjKN5E^(OccJZ%v1bGomT3_~F7m@tpmz)q_N zUEDo`5+W3JRJL^_ZZj?yQdxt_!x~WF^Ah}XM5O~POc1;M8?@!6D^;MFgU0v?V2h$xjH>7&xp1g54BpROTR zl(`g{XBJ{q0;j~%Udsz(;;`~j=6jWXJ0d#pm>QUkq0ajS=|D_HjRXyCRrQS*uo?w?{)pN=G75 z8%$9myV29_Knl@bZVx{@kpPyRq=g#un|ADn^Iw23d=h#EN8s}S9`^1wl1Mv0p`s`Z zTfvCwN3`uQlhdSrg42G6Tr)W7Ilj1H;qAq8WA35ZV6#jPV0(Hkq=EOM75U*p#u_Ej z|H(5b$J_nY`cgd@Q2$&}&4$od{M=s*T^!l`x!gVUS1u;L!Q>J^t|>Dkso=&{Js`ag zehM@Fv@J8wV+41Kx1_-J8{QP}O!|U$g9)HGxQ?RXb@p@3+XV-)cw`p{g=_>)l^ZAs z=@fEaj)(4Kah-0jE!F(%1IJhx`TNB7wy1peu3n&fZ@Hrz3}Wt5)XJA8zKZI4beVfw z1}O*&cz;B3&xDuDv@?lAPOUsLZU>g2-2-ylFHPh-sEfmxp(i^-XVtO`;kZoam6)IR z5%m~MDR)0yKFoy!OtWcHCo^M8BV_o@TT#6-N80GkMTNFo2sfO_baPqCC8G;|y1y)a zcQ(i@g=D~llvvxnRrL#$`$EVd5hEUI~iO_KKU&yu;+?9o5a6g zM7)Ds zn#m0!=`vpzDsOg$3MF9@i@IQg5mjD$pKrs<;#O#1Z(E{@RP`rn61E;>u?t$LoqqH0 zLi(_u#^p`!Y(#3hZUkD^`0aWG7y-e6|I$r>4mWyYp%9_JdL)wuCtj|M?Xeph8 z+Z0%Upf-3cNCrY#BUfDh07IpWk*KjuK^vORY4>o+l&Zr)$*{>@Jk0bGURGQgDQMFz z()s4P;v@rU5PLGVMk?`n_DQ+-N`KKEO!iy{1%jP=zY%-=onzMcUD1*_04;P9=Zk+t zRm?0;$1cnxhFo1+06Ml*WmrDKfbyMf{tGJcm-%3oGN>?ljRlW8y)gmD5}M$rJghlG za8sNTzrJj)B@?H)bc-5Ji94becPA{<#s}9#DvA02C0^AwDq# z$_|QEIqbDj8YYrTtDZJKLNdf^kzyW}IMx>t&s(`L3$Xt%bMj^_ZI>V12hN{n>pO9R zE1;GldbFdrY~HSd_jxIM>FI*gvlqrRR_C>29Ig4^&dcJib*fI#M)q?0xqb9$5}SR( z1{iJlOb~RiHB3jCg=L*>{AxHzxmTg9V1GgbQ}i}bU&EKgc+1! z$TwV`NYpB4{dsR^H8nhN&8wKj;43OBI!yX9qGJQh*gh6>W~7+R=`>0DcoD(L+6o{S zNeT>TH%GRpI%mingyq>S+g%Jd$L(}Nq3g_3c3F>p^Y}&{9)j_sbSYg0Zjay8*Is3= z^AP95DK(U@L~(-mbQ4j+?sYv@JXW8W^m@(`i*36@E&|!HuB%MQAy_LMT`Q2RZT)nG zkTZ&;#s)&2JBLv_5wzYcpbGun-G(c$#>>2elIIrkaopYlDV)Krz4W~$xmTvUsA(oS zb+h!jk)7SCpeQ4Ypl)aIa5|y&&D}oT^w#-DlXKaED&RvcQNwmP?{~PVygO&vR_fs7 zBI_vfWD6^~Lg$UqIirJy`98MOyfk;aS&Mc31s!n;UqdCW(XJNgHALQnRCCP{pO={t zFQb3eLTHh&NLM~*HaT&wK>Q@PLG1Etbm1e#xI?_zo|?z#OylL+(tsx#z$0IN1qOM! zaYz#i`Qznm@(Z;-r?P6Hk4j8nrLmdmG|a;^dF+Q5M*nf5EVzUvil5KVSeQwpXMU&$ zI`HR`thBmAKkw|PNb1(UsyIY=w6KrKSNhbI#tQ)c7%x!5+>jjn2bN=*d1^HSgdlgVxBM~P%8~AE}Fa>m#3vlV+H)j@{ zn|TR6N{a!B*vMqzef z=ni|g6yfAJv1pLa=j&?NPrDoB=kF&X0WEgSg`cr)j-P5|K~$z5V?1Gs{-KK2;(2(X5dBg#fsaMSW4Vn)3O zfP;A~bl`7KJaQgc_@!*RWu8zgkd;3O(V({UR@U?xb+sicOn;t*w6z;YGB|mXiv`sj zxL#md8>;WsYt}f$vyqTvvVqp3_S-AAf6$+yc8GD^glD*56B4;9Dx}7nu*;PVe8){| zG|&!nkN2&h2tj8)AbtEmQ%eE=u8qOO+ImPdaBT|m#{foaBjHZCAa(?X@}qoNtasz_$$K(}DvtUhm5sbt*}G+j^k zNbXT3v|nOr_F&jgSow#37pSp}nTN?E2Zy_lmrX2ix`DFpfH$mozH8b+v6D;j118q& z$-$p%cUWI`Cg&DXpM<$L-%pj-CD*i#C8*b;c!rRYcq#M!GXx7m?(rQl=5!?~nWo`4 zTw(Rw8Zxwx`31Sfx#J(CJEg6Q6teZO*N38cqEBm5u!DxA;aO3Xyu&r77|a-#KU< zm?LL)!hOpmd{@_XyYi*)GKQE5Iht#g^^+Zqf>ju0plQ3a>;O2$CFdG&VG_c8$6;j&omPAI^VE2Gr>!L)HXFP$XYD- zc&aR7$yRVpkLb1hFr@5u7lr}9j!oh5tc9W15+61whoLPGV{(5B|+u?XR(%^%KwDRreZHA}-#41i0yXfI!wwU>;HS@4>_3$4; zycgYk4C2%~)MDhlVPZu%@VY}0RUEdL#+99-3hHwMze4@f59dGD{}}4;9{z7B^X~}#MdUC1KMDP3 zKkL5}`uz`$_3z^Tmn8c)+Uj53t^ZErL;XMV@>hNoWnh5+OdixfCKv($2qF0A?SBDx CZ&8N; literal 0 HcmV?d00001 diff --git a/powerboardcode/powerboard/powerboard.h b/powerboardcode/powerboard/powerboard.h new file mode 100644 index 0000000..20c1d29 --- /dev/null +++ b/powerboardcode/powerboard/powerboard.h @@ -0,0 +1,67 @@ + +#include "Energia.h" +#include "RoveComm.h" +//12V Current Sensing +#define MULTIMEDIA_SENSE PD4 +#define NAV_SENSE PD0 +#define GIMBAL_ACT_SENSE PD1 +#define DRIVE_SENSE PK3 +#define SCISENSOR_ACT_SENSE PD2 +#define NETSWITCH_SENSE PB5 +#define CAM1_SENSE PB4 +#define CAM2_SENSE PK0 +#define BBB_SENSE PD7 +#define AUX_LOG_SENSE PK2 + +//12V CTL +#define MULTIMEDIA_CTL PL1 +#define NAV_CTL PN4 +#define GIMBAL_ACT_CTL PA4 +#define GIMBAL_LOG_CTL PG0 +#define DRIVE_CTL PL2 +#define SCISENSOR_ACT_CTL PL3 +#define SCISENSOR_LOG_CTL PF3 +#define NETSWITCH_CTL PL0 +#define CAM1_CTL PF1 +#define CAM2_CTL PF2 +#define BBB_CTL PN5 +#define AUX_LOG_CTL PB2 +#define SPARE_CTL PL5 + +//PACK CURRENT +#define P_MOTOR1_SENSE PE4 +#define P_MOTOR2_SENSE PE5 +#define P_MOTOR3_SENSE PD3 +#define P_MOTOR4_SENSE PE0 +#define P_MOTOR5_SENSE PE1 +#define P_MOTOR6_SENSE PE2 +#define P_MOTOR7_SENSE PE3 +#define P_POE_SENSE PD5 +#define P_AUX_SENSE PK1 + + +//PACK BUSSES +#define P_MOTOR1_CTL PC4 +#define P_MOTOR2_CTL PC5 +#define P_MOTOR3_CTL PC6 +#define P_MOTOR4_CTL PC7 +#define P_MOTOR5_CTL PP0 +#define P_MOTOR6_CTL PP1 +#define P_MOTOR7_CTL PQ0 +#define P_POE_CTL PP4 +#define P_AUX_CTL PL4 + +#define MOTOR_DELAY 5000 +#define DRIVE_DELAY 5000 + +uint8_t currentSense12V[] = {MULTIMEDIA_SENSE, NAV_SENSE, GIMBAL_ACT_SENSE, DRIVE_SENSE, SCISENSOR_ACT_SENSE, NETSWITCH_SENSE, CAM1_SENSE, CAM2_SENSE, BBB_SENSE, AUX_LOG_SENSE}; +uint8_t actuation12V[5] = {GIMBAL_ACT_CTL, AUX_LOG_CTL, SPARE_CTL, MULTIMEDIA_CTL, SCISENSOR_ACT_CTL}; +uint8_t logic12V[8] = {GIMBAL_LOG_CTL, DRIVE_CTL, NETSWITCH_CTL, NAV_CTL, CAM1_CTL, CAM2_CTL, BBB_CTL, SCISENSOR_LOG_CTL}; +//uint8_t drivePack = P_DRIVE_CTL; +uint8_t currentSensePack[9] = {P_MOTOR1_SENSE, P_MOTOR2_SENSE, P_MOTOR3_SENSE, P_MOTOR4_SENSE, P_MOTOR5_SENSE, P_MOTOR6_SENSE, P_MOTOR7_SENSE, P_POE_SENSE, P_AUX_SENSE}; +uint8_t bussesPack[4] = {P_POE_CTL, , P_AUX_CTL, }; +uint8_t bussesMotor[7] = {P_MOTOR1_CTL, P_MOTOR2_CTL, P_MOTOR3_CTL, P_MOTOR4_CTL, P_MOTOR5_CTL, P_MOTOR6_CTL, P_MOTOR7_CTL}; + +RoveCommEthernet RoveComm; +rovecomm_packet packet; +EthernetServer TCPServer(RC_ROVECOMM_POWERBOARD_PORT); diff --git a/powerboardcode/powerboard/powerboard.ino b/powerboardcode/powerboard/powerboard.ino new file mode 100644 index 0000000..6b04020 --- /dev/null +++ b/powerboardcode/powerboard/powerboard.ino @@ -0,0 +1,216 @@ +#include "powerboard.h" +void setup() +{ + Serial.begin(9600); + setPins(); + setPinStates(); + Serial.begin(9600); + RoveComm.begin(RC_POWERBOARD_FOURTHOCTET, &TCPServer); + delay(100); +} + +void loop() +{ + packet = RoveComm.read(); + if(packet.data_id != 0) + { + switch(packet.data_id) + { + case RC_POWERBOARD_MOTORBUSENABLE_DATA_ID: + Serial.println("Enable/Disable Motor Busses"); + for(int i = 0; i < 7; i++) //way i<7 + { + if (packet.data[0] & 1< Date: Fri, 25 Feb 2022 22:03:08 -0600 Subject: [PATCH 07/14] removed obsolete code and changed loop parameters to match current number of ports --- powerboardcode/powerboard/powerboard.h | 56 ++++++++++++------------ powerboardcode/powerboard/powerboard.ino | 48 +++++++++----------- 2 files changed, 50 insertions(+), 54 deletions(-) diff --git a/powerboardcode/powerboard/powerboard.h b/powerboardcode/powerboard/powerboard.h index 20c1d29..233d2e4 100644 --- a/powerboardcode/powerboard/powerboard.h +++ b/powerboardcode/powerboard/powerboard.h @@ -2,6 +2,7 @@ #include "Energia.h" #include "RoveComm.h" //12V Current Sensing +/* #define MULTIMEDIA_SENSE PD4 #define NAV_SENSE PD0 #define GIMBAL_ACT_SENSE PD1 @@ -12,22 +13,24 @@ #define CAM2_SENSE PK0 #define BBB_SENSE PD7 #define AUX_LOG_SENSE PK2 +*/ //12V CTL -#define MULTIMEDIA_CTL PL1 -#define NAV_CTL PN4 -#define GIMBAL_ACT_CTL PA4 -#define GIMBAL_LOG_CTL PG0 -#define DRIVE_CTL PL2 -#define SCISENSOR_ACT_CTL PL3 -#define SCISENSOR_LOG_CTL PF3 -#define NETSWITCH_CTL PL0 -#define CAM1_CTL PF1 -#define CAM2_CTL PF2 -#define BBB_CTL PN5 -#define AUX_LOG_CTL PB2 -#define SPARE_CTL PL5 +#define MULTIMEDIA_CTL PL_1 +#define NAV_CTL PN_4 +#define GIMBAL_ACT_CTL PA_4 +#define GIMBAL_LOG_CTL PG_0 +#define DRIVE_CTL PL_2 +#define SCISENSOR_ACT_CTL PL_3 +#define SCISENSOR_LOG_CTL PF_3 +#define NETSWITCH_CTL PL_0 +#define CAM1_CTL PF_1 +#define CAM2_CTL PF_2 +#define BBB_CTL PN_5 +#define AUX_LOG_CTL PB_2 +#define SPARE_CTL PL_5 +/* //PACK CURRENT #define P_MOTOR1_SENSE PE4 #define P_MOTOR2_SENSE PE5 @@ -38,28 +41,27 @@ #define P_MOTOR7_SENSE PE3 #define P_POE_SENSE PD5 #define P_AUX_SENSE PK1 - +*/ //PACK BUSSES -#define P_MOTOR1_CTL PC4 -#define P_MOTOR2_CTL PC5 -#define P_MOTOR3_CTL PC6 -#define P_MOTOR4_CTL PC7 -#define P_MOTOR5_CTL PP0 -#define P_MOTOR6_CTL PP1 -#define P_MOTOR7_CTL PQ0 -#define P_POE_CTL PP4 -#define P_AUX_CTL PL4 +#define P_MOTOR1_CTL PC_4 +#define P_MOTOR2_CTL PC_5 +#define P_MOTOR3_CTL PC_6 +#define P_MOTOR4_CTL PC_7 +#define P_MOTOR5_CTL PP_0 +#define P_MOTOR6_CTL PP_1 +#define P_MOTOR7_CTL PQ_0 +#define P_POE_CTL PP_4 +#define P_AUX_CTL PL_4 #define MOTOR_DELAY 5000 #define DRIVE_DELAY 5000 -uint8_t currentSense12V[] = {MULTIMEDIA_SENSE, NAV_SENSE, GIMBAL_ACT_SENSE, DRIVE_SENSE, SCISENSOR_ACT_SENSE, NETSWITCH_SENSE, CAM1_SENSE, CAM2_SENSE, BBB_SENSE, AUX_LOG_SENSE}; +//uint8_t currentSense12V[] = {MULTIMEDIA_SENSE, NAV_SENSE, GIMBAL_ACT_SENSE, DRIVE_SENSE, SCISENSOR_ACT_SENSE, NETSWITCH_SENSE, CAM1_SENSE, CAM2_SENSE, BBB_SENSE, AUX_LOG_SENSE}; uint8_t actuation12V[5] = {GIMBAL_ACT_CTL, AUX_LOG_CTL, SPARE_CTL, MULTIMEDIA_CTL, SCISENSOR_ACT_CTL}; uint8_t logic12V[8] = {GIMBAL_LOG_CTL, DRIVE_CTL, NETSWITCH_CTL, NAV_CTL, CAM1_CTL, CAM2_CTL, BBB_CTL, SCISENSOR_LOG_CTL}; -//uint8_t drivePack = P_DRIVE_CTL; -uint8_t currentSensePack[9] = {P_MOTOR1_SENSE, P_MOTOR2_SENSE, P_MOTOR3_SENSE, P_MOTOR4_SENSE, P_MOTOR5_SENSE, P_MOTOR6_SENSE, P_MOTOR7_SENSE, P_POE_SENSE, P_AUX_SENSE}; -uint8_t bussesPack[4] = {P_POE_CTL, , P_AUX_CTL, }; +//uint8_t currentSensePack[9] = {P_MOTOR1_SENSE, P_MOTOR2_SENSE, P_MOTOR3_SENSE, P_MOTOR4_SENSE, P_MOTOR5_SENSE, P_MOTOR6_SENSE, P_MOTOR7_SENSE, P_POE_SENSE, P_AUX_SENSE}; +uint8_t bussesPack[2] = {P_POE_CTL, P_AUX_CTL, }; uint8_t bussesMotor[7] = {P_MOTOR1_CTL, P_MOTOR2_CTL, P_MOTOR3_CTL, P_MOTOR4_CTL, P_MOTOR5_CTL, P_MOTOR6_CTL, P_MOTOR7_CTL}; RoveCommEthernet RoveComm; diff --git a/powerboardcode/powerboard/powerboard.ino b/powerboardcode/powerboard/powerboard.ino index 6b04020..34bb7d1 100644 --- a/powerboardcode/powerboard/powerboard.ino +++ b/powerboardcode/powerboard/powerboard.ino @@ -34,7 +34,7 @@ void loop() } } break; - case RC_POWERBOARD_12VACTBUSENABLE_DATA_ID: + case RC_POWERBOARD_TWELVEVACTBUSENABLE_DATA_ID: Serial.println("Enable/Disable 12V Actuation Busses"); for(int i = 0; i < 5; i++) { @@ -52,7 +52,7 @@ void loop() } } break; - case RC_POWERBOARD_12VLOGICBUSENABLE_DATA_ID: + case RC_POWERBOARD_TWELVEVLOGICBUSENABLE_DATA_ID: Serial.println("Enable/Disable 12V Logic Busses"); for(int i = 0; i < 8; i++) { @@ -70,9 +70,9 @@ void loop() } } break; - case RC_POWERBOARD_30VBUSENABLE_DATA_ID: + case RC_POWERBOARD_THIRTYVBUSENABLE_DATA_ID: Serial.println("Enable/Disable 30V Busses"); - for(int i = 0; i < 4; i++) + for(int i = 0; i < 2; i++) { if (packet.data[0] & 1< Date: Sun, 24 Apr 2022 00:58:35 -0500 Subject: [PATCH 08/14] Beta Test for rev2 boards --- powerboardcode/powerboard/powerboard.h | 47 ++++++++++++------------ powerboardcode/powerboard/powerboard.ino | 2 +- 2 files changed, 24 insertions(+), 25 deletions(-) diff --git a/powerboardcode/powerboard/powerboard.h b/powerboardcode/powerboard/powerboard.h index 233d2e4..07c2e7c 100644 --- a/powerboardcode/powerboard/powerboard.h +++ b/powerboardcode/powerboard/powerboard.h @@ -1,5 +1,4 @@ - -#include "Energia.h" +#pragma once #include "RoveComm.h" //12V Current Sensing /* @@ -16,19 +15,19 @@ */ //12V CTL -#define MULTIMEDIA_CTL PL_1 -#define NAV_CTL PN_4 -#define GIMBAL_ACT_CTL PA_4 -#define GIMBAL_LOG_CTL PG_0 -#define DRIVE_CTL PL_2 -#define SCISENSOR_ACT_CTL PL_3 -#define SCISENSOR_LOG_CTL PF_3 -#define NETSWITCH_CTL PL_0 -#define CAM1_CTL PF_1 -#define CAM2_CTL PF_2 -#define BBB_CTL PN_5 -#define AUX_LOG_CTL PB_2 -#define SPARE_CTL PL_5 +#define MULTIMEDIA_CTL 2 +#define NAV_CTL 1 +#define GIMBAL_ACT_CTL 14 +#define GIMBAL_LOG_CTL 3 +#define DRIVE_CTL 4 +#define SCISENSOR_ACT_CTL 16 +#define SCISENSOR_LOG_CTL 17 +#define NETSWITCH_CTL 18 +#define CAM1_CTL 6 +#define CAM2_CTL 5 +#define BBB_CTL 7 +#define AUX_LOG_CTL 8 +#define SPARE_CTL 37 /* //PACK CURRENT @@ -44,15 +43,15 @@ */ //PACK BUSSES -#define P_MOTOR1_CTL PC_4 -#define P_MOTOR2_CTL PC_5 -#define P_MOTOR3_CTL PC_6 -#define P_MOTOR4_CTL PC_7 -#define P_MOTOR5_CTL PP_0 -#define P_MOTOR6_CTL PP_1 -#define P_MOTOR7_CTL PQ_0 -#define P_POE_CTL PP_4 -#define P_AUX_CTL PL_4 +#define P_MOTOR1_CTL 33 +#define P_MOTOR2_CTL 32 +#define P_MOTOR3_CTL 34 +#define P_MOTOR4_CTL 31 +#define P_MOTOR5_CTL 35 +#define P_MOTOR6_CTL 30 +#define P_MOTOR7_CTL 36 +#define P_POE_CTL 29 +#define P_AUX_CTL 8 #define MOTOR_DELAY 5000 #define DRIVE_DELAY 5000 diff --git a/powerboardcode/powerboard/powerboard.ino b/powerboardcode/powerboard/powerboard.ino index 34bb7d1..8e55fef 100644 --- a/powerboardcode/powerboard/powerboard.ino +++ b/powerboardcode/powerboard/powerboard.ino @@ -5,7 +5,7 @@ void setup() setPins(); setPinStates(); Serial.begin(9600); - RoveComm.begin(RC_POWERBOARD_FOURTHOCTET, &TCPServer); + RoveComm.begin(RC_POWERBOARD_FOURTHOCTET, &TCPServer, RC_ROVECOMM_POWERBOARD_MAC); delay(100); } From 9aaad170f8beafdcfa8e6784aeb14135de7ee2cc Mon Sep 17 00:00:00 2001 From: bdavis2020 <70910468+bdavis2020@users.noreply.github.com> Date: Mon, 25 Apr 2022 03:14:06 -0500 Subject: [PATCH 09/14] fixed teensy pins, removed unused functionality, changed motor delay to 2 seconds between each motor toggle in setup, created variable to store packet data, created constants for number of ports being toggled as arrays --- powerboardcode/powerboard/powerboard.h | 53 ++++----- powerboardcode/powerboard/powerboard.ino | 132 +++++++++-------------- 2 files changed, 81 insertions(+), 104 deletions(-) diff --git a/powerboardcode/powerboard/powerboard.h b/powerboardcode/powerboard/powerboard.h index 07c2e7c..6730786 100644 --- a/powerboardcode/powerboard/powerboard.h +++ b/powerboardcode/powerboard/powerboard.h @@ -1,5 +1,11 @@ -#pragma once +#ifndef POWERBOARD_H +#define POWERBOARD_H #include "RoveComm.h" + +// Numbers of Toggleable Ports +#define NUM_12V_PORTS 8 // Everything except Aux cuz Manifest +#define NUM_MOTORS 7 + //12V Current Sensing /* #define MULTIMEDIA_SENSE PD4 @@ -15,19 +21,15 @@ */ //12V CTL -#define MULTIMEDIA_CTL 2 +#define SPARE_CTL 0 #define NAV_CTL 1 -#define GIMBAL_ACT_CTL 14 -#define GIMBAL_LOG_CTL 3 +#define MULTIMEDIA_CTL 2 +#define GIMBAL_CTL 3 #define DRIVE_CTL 4 -#define SCISENSOR_ACT_CTL 16 -#define SCISENSOR_LOG_CTL 17 -#define NETSWITCH_CTL 18 -#define CAM1_CTL 6 #define CAM2_CTL 5 +#define CAM1_CTL 6 #define BBB_CTL 7 -#define AUX_LOG_CTL 8 -#define SPARE_CTL 37 +#define AUX_CTL 8 /* //PACK CURRENT @@ -43,26 +45,27 @@ */ //PACK BUSSES -#define P_MOTOR1_CTL 33 -#define P_MOTOR2_CTL 32 -#define P_MOTOR3_CTL 34 -#define P_MOTOR4_CTL 31 -#define P_MOTOR5_CTL 35 -#define P_MOTOR6_CTL 30 -#define P_MOTOR7_CTL 36 -#define P_POE_CTL 29 -#define P_AUX_CTL 8 +#define POE_CTL 29 +#define PACK_SPARE_CTL 37 -#define MOTOR_DELAY 5000 -#define DRIVE_DELAY 5000 +#define MOTOR1_CTL 33 +#define MOTOR2_CTL 32 +#define MOTOR3_CTL 34 +#define MOTOR4_CTL 31 +#define MOTOR5_CTL 35 +#define MOTOR6_CTL 30 +#define MOTORS_CTL 36 + +#define MOTOR_DELAY 2000 //uint8_t currentSense12V[] = {MULTIMEDIA_SENSE, NAV_SENSE, GIMBAL_ACT_SENSE, DRIVE_SENSE, SCISENSOR_ACT_SENSE, NETSWITCH_SENSE, CAM1_SENSE, CAM2_SENSE, BBB_SENSE, AUX_LOG_SENSE}; -uint8_t actuation12V[5] = {GIMBAL_ACT_CTL, AUX_LOG_CTL, SPARE_CTL, MULTIMEDIA_CTL, SCISENSOR_ACT_CTL}; -uint8_t logic12V[8] = {GIMBAL_LOG_CTL, DRIVE_CTL, NETSWITCH_CTL, NAV_CTL, CAM1_CTL, CAM2_CTL, BBB_CTL, SCISENSOR_LOG_CTL}; +uint8_t twelveVoltBusses[NUM_12V_PORTS] = {GIMBAL_CTL, DRIVE_CTL, MULTIMEDIA_CTL, NAV_CTL, CAM1_CTL, CAM2_CTL, BBB_CTL, SPARE_CTL}; //uint8_t currentSensePack[9] = {P_MOTOR1_SENSE, P_MOTOR2_SENSE, P_MOTOR3_SENSE, P_MOTOR4_SENSE, P_MOTOR5_SENSE, P_MOTOR6_SENSE, P_MOTOR7_SENSE, P_POE_SENSE, P_AUX_SENSE}; -uint8_t bussesPack[2] = {P_POE_CTL, P_AUX_CTL, }; -uint8_t bussesMotor[7] = {P_MOTOR1_CTL, P_MOTOR2_CTL, P_MOTOR3_CTL, P_MOTOR4_CTL, P_MOTOR5_CTL, P_MOTOR6_CTL, P_MOTOR7_CTL}; +uint8_t motorBusses[NUM_MOTORS] = {MOTOR1_CTL, MOTOR2_CTL, MOTOR3_CTL, MOTOR4_CTL, MOTOR5_CTL, MOTOR6_CTL, MOTORS_CTL}; RoveCommEthernet RoveComm; rovecomm_packet packet; +uint8_t* data; EthernetServer TCPServer(RC_ROVECOMM_POWERBOARD_PORT); + +#endif \ No newline at end of file diff --git a/powerboardcode/powerboard/powerboard.ino b/powerboardcode/powerboard/powerboard.ino index 8e55fef..2367b7d 100644 --- a/powerboardcode/powerboard/powerboard.ino +++ b/powerboardcode/powerboard/powerboard.ino @@ -1,91 +1,81 @@ #include "powerboard.h" void setup() { - Serial.begin(9600); + Serial.begin(115200); setPins(); setPinStates(); - Serial.begin(9600); + Serial.begin(115200); RoveComm.begin(RC_POWERBOARD_FOURTHOCTET, &TCPServer, RC_ROVECOMM_POWERBOARD_MAC); - delay(100); } void loop() { packet = RoveComm.read(); + data = (uint8_t*)packet.data; if(packet.data_id != 0) { switch(packet.data_id) { case RC_POWERBOARD_MOTORBUSENABLE_DATA_ID: - Serial.println("Enable/Disable Motor Busses"); - for(int i = 0; i < 7; i++) //way i<7 + Serial.println("Enable/Disable Motor Busses: "); + for(int i = 0; i < NUM_MOTORS; i++) { - if (packet.data[0] & 1< Date: Mon, 25 Apr 2022 03:15:18 -0500 Subject: [PATCH 10/14] removed old current sense bs, needs to be rewritten soon --- powerboardcode/powerboard/powerboard.ino | 63 +----------------------- 1 file changed, 1 insertion(+), 62 deletions(-) diff --git a/powerboardcode/powerboard/powerboard.ino b/powerboardcode/powerboard/powerboard.ino index 2367b7d..0791b6d 100644 --- a/powerboardcode/powerboard/powerboard.ino +++ b/powerboardcode/powerboard/powerboard.ino @@ -120,65 +120,4 @@ void setPinStates() delay(MOTOR_DELAY); digitalWrite(motorBusses[i], HIGH); } -} -/* -void read_CurrentSense() -{ - int motor1_sense = analogRead(P_MOTOR1_SENSE); - int motor2_sense = analogRead(P_MOTOR2_SENSE); - int motor3_sense = analogRead(P_MOTOR3_SENSE); - int motor4_sense = analogRead(P_MOTOR4_SENSE); - int motor5_sense = analogRead(P_MOTOR5_SENSE); - int motor6_sense = analogRead(P_MOTOR6_SENSE); - int motor7_sense = analogRead(P_MOTOR7_SENSE); - int poe_sense = analogRead(P_POE_SENSE); - int aux_sense = analogRead(P_AUX_SENSE); - int multimedia_sense = analogRead(MULTIMEDIA_SENSE); - int nav_sense = analogRead(NAV_SENSE); - int gimbal_sense = analogRead(GIMBAL_ACT_SENSE); - int drive_sense = analogRead(DRIVE_SENSE); - int scisensor_sense = analogRead(SCISENSOR_ACT_SENSE); - int netswitch_sense = analogRead(NETSWITCH_SENSE); - int cam1_sense = analogRead(CAM1_SENSE); - int cam2_sense = analogRead(CAM2_SENSE); - int bbb_sense = analogRead(BBB_SENSE); - int aux_log_sense = analogRead(AUX_LOG_SENSE); - - Serial.println("Motor 1 sense: "); - Serial.println(motor1_sense); - Serial.println("Motor 2 sense: "); - Serial.println(motor2_sense); - Serial.println("Motor 3 sense: "); - Serial.println(motor3_sense); - Serial.println("Motor 4 sense: "); - Serial.println(motor4_sense); - Serial.println("Motor 5 sense: "); - Serial.println(motor5_sense); - Serial.println("Motor 6 sense: "); - Serial.println(motor6_sense); - Serial.println("Motor 7 sense: "); - Serial.println(motor7_sense); - Serial.println("POE sense: "); - Serial.println(poe_sense); - Serial.println("Aux sense: "); - Serial.println(aux_sense); - Serial.println("Multimedia sense: "); - Serial.println(multimedia_sense); - Serial.println("Nav sense: "); - Serial.println(nav_sense); - Serial.println("Gimbal sense: "); - Serial.println(gimbal_sense); - Serial.println("Drive sense: "); - Serial.println(drive_sense); - Serial.println("SciSensor sense: "); - Serial.println(scisensor_sense); - Serial.println("Cam 1 sense: "); - Serial.println(cam1_sense); - Serial.println("Cam 2 sense: "); - Serial.println(cam2_sense); - Serial.println("BBB sense: "); - Serial.println(bbb_sense); - Serial.println("Aux Log sense: "); - Serial.println(Aux_Log_sense); -} -*/ \ No newline at end of file +} \ No newline at end of file From b8d848650e4457ae1f2a7045181ec34e70949ba9 Mon Sep 17 00:00:00 2001 From: Grant Brinker Date: Tue, 26 Apr 2022 00:41:52 -0500 Subject: [PATCH 11/14] Added current sensing, overcurrent, and telemetry --- powerboardcode/powerboard/powerboard.h | 82 +++++++++++++++++------- powerboardcode/powerboard/powerboard.ino | 81 +++++++++++++++++++++++ 2 files changed, 140 insertions(+), 23 deletions(-) diff --git a/powerboardcode/powerboard/powerboard.h b/powerboardcode/powerboard/powerboard.h index 6730786..bd2bb6d 100644 --- a/powerboardcode/powerboard/powerboard.h +++ b/powerboardcode/powerboard/powerboard.h @@ -6,19 +6,26 @@ #define NUM_12V_PORTS 8 // Everything except Aux cuz Manifest #define NUM_MOTORS 7 +IntervalTimer Telemetry; + //12V Current Sensing -/* -#define MULTIMEDIA_SENSE PD4 -#define NAV_SENSE PD0 -#define GIMBAL_ACT_SENSE PD1 -#define DRIVE_SENSE PK3 -#define SCISENSOR_ACT_SENSE PD2 -#define NETSWITCH_SENSE PB5 -#define CAM1_SENSE PB4 -#define CAM2_SENSE PK0 -#define BBB_SENSE PD7 -#define AUX_LOG_SENSE PK2 -*/ + +#define MULTIMEDIA_SENSE A11 +#define NAV_SENSE A12 +#define GIMBAL_ACT_SENSE A10 +#define DRIVE_SENSE A0 +#define CAM1_SENSE A2 +#define CAM2_SENSE A1 +#define BBB_SENSE A3 +#define AUX_SENSE A4 +#define SPARE_12V_SENSE A13 + +#define OVERCURRENT_PACK 19500 //mA +#define OVERCURRENT_12V 4800 //mA +#define CURRENT_ADC_MIN 0 +#define CURRENT_ADC_MAX 0 +#define CURRENT_mA_MIN 0 +#define CURRENT_mA_MAX 0 //12V CTL #define SPARE_CTL 0 @@ -31,18 +38,47 @@ #define BBB_CTL 7 #define AUX_CTL 8 -/* + //PACK CURRENT -#define P_MOTOR1_SENSE PE4 -#define P_MOTOR2_SENSE PE5 -#define P_MOTOR3_SENSE PD3 -#define P_MOTOR4_SENSE PE0 -#define P_MOTOR5_SENSE PE1 -#define P_MOTOR6_SENSE PE2 -#define P_MOTOR7_SENSE PE3 -#define P_POE_SENSE PD5 -#define P_AUX_SENSE PK1 -*/ +#define P_MOTOR1_SENSE A14 +#define P_MOTOR2_SENSE A15 +#define P_MOTOR3_SENSE A16 +#define P_MOTOR4_SENSE A17 +#define P_MOTOR5_SENSE A5 +#define P_MOTOR6_SENSE A6 +#define P_MOTOR7_SENSE A7 +#define P_POE_SENSE A8 + +// Current sensing variables + +float MOTORBUSPINS[7] = {P_MOTOR1_SENSE, P_MOTOR2_SENSE, P_MOTOR3_SENSE, P_MOTOR4_SENSE, P_MOTOR5_SENSE, P_MOTOR6_SENSE, P_MOTOR7_SENSE}; +float TWELVELOGICBUSPINS[8] = {GIMBAL_ACT_SENSE, DRIVE_SENSE, MULTIMEDIA_SENSE, NAV_SENSE, CAM1_SENSE, CAM2_SENSE, BBB_SENSE, SPARE_12V_SENSE}; + +float P_MOTOR1_CURRENT = 0; +float P_MOTOR2_CURRENT = 0; +float P_MOTOR3_CURRENT = 0; +float P_MOTOR4_CURRENT = 0; +float P_MOTOR5_CURRENT = 0; +float P_MOTOR6_CURRENT = 0; +float P_MOTOR7_CURRENT = 0; + +float AUX_CURRENT = 0; + +float GIMBAL_ACT_CURRENT = 0; +float DRIVE_CURRENT = 0; +float MULTIMEDIA_CURRENT = 0; +float NAV_CURRENT = 0; +float CAM1_CURRENT = 0; +float CAM2_CURRENT = 0; +float BBB_CURRENT = 0; +float SPARE_12V_CURRENT = 0; + +float MOTORBUSCURRENTS[7] = {P_MOTOR1_CURRENT, P_MOTOR2_CURRENT, P_MOTOR3_CURRENT, P_MOTOR4_CURRENT, P_MOTOR5_CURRENT, P_MOTOR6_CURRENT, P_MOTOR7_CURRENT}; +float TWELVELOGICBUSCURRENTS[8] = {GIMBAL_ACT_CURRENT, DRIVE_CURRENT, MULTIMEDIA_CURRENT, NAV_CURRENT, CAM1_CURRENT, CAM2_CURRENT, BBB_CURRENT, SPARE_12V_CURRENT}; + +uint8_t motorOverCurrent = 0; +uint8_t twelveActOverCurrent = 0; +uint8_t twelveLogicOverCurrent = 0; //PACK BUSSES #define POE_CTL 29 diff --git a/powerboardcode/powerboard/powerboard.ino b/powerboardcode/powerboard/powerboard.ino index 0791b6d..fee31b1 100644 --- a/powerboardcode/powerboard/powerboard.ino +++ b/powerboardcode/powerboard/powerboard.ino @@ -2,6 +2,7 @@ void setup() { Serial.begin(115200); + Telemetry.begin(telemetry, 150000); setPins(); setPinStates(); Serial.begin(115200); @@ -80,6 +81,9 @@ void loop() break; } } + + measureCurrent(); + overCurrent(); } void setPins() @@ -120,4 +124,81 @@ void setPinStates() delay(MOTOR_DELAY); digitalWrite(motorBusses[i], HIGH); } +} + +float senseCurrent(const uint8_t sensePin) +{ + float meas_current = analogRead(sensePin); + float current = map(meas_current, CURRENT_ADC_MIN, CURRENT_ADC_MAX, CURRENT_mA_MIN, CURRENT_mA_MAX); + return current; +} + +void measureCurrent() +{ + for (uint8_t i = 0; i < 7; i++) + { + MOTORBUSCURRENTS[i] = senseCurrent(MOTORBUSPINS[i]); + + if (MOTORBUSCURRENTS[i] > OVERCURRENT_PACK) + { + motorOverCurrent |= (1 << i); + } + else + { + motorOverCurrent &= !(1 << i); + } + } + + AUX_CURRENT = senseCurrent(AUX_SENSE); + if (AUX_CURRENT > OVERCURRENT_12V) + { + twelveActOverCurrent = 1; + } + else + { + twelveActOverCurrent = 0; + } + + for (uint8_t i = 0; i < 8; i++) + { + TWELVELOGICBUSCURRENTS[i] = senseCurrent(TWELVELOGICBUSPINS[i]); + + if (TWELVELOGICBUSCURRENTS[i] > OVERCURRENT_12V) + { + twelveLogicOverCurrent |= (1 << i); + } + else + { + twelveLogicOverCurrent &= !(1 << i); + } + } + return; +} + +void overCurrent() +{ + if (motorOverCurrent) + { + RoveComm.write(RC_POWERBOARD_MOTORBUSOVERCURRENT_DATA_ID, RC_POWERBOARD_MOTORBUSOVERCURRENT_DATA_COUNT, motorOverCurrent); + } + if (twelveActOverCurrent) + { + RoveComm.write(RC_POWERBOARD_TWELVEVACTBUSOVERCURRENT_DATA_ID, RC_POWERBOARD_TWELVEVACTBUSOVERCURRENT_DATA_COUNT, twelveActOverCurrent); + } + if (twelveLogicOverCurrent) + { + RoveComm.write(RC_POWERBOARD_TWELVEVLOGICBUSOVERCURRENT_DATA_ID, RC_POWERBOARD_TWELVEVLOGICBUSOVERCURRENT_DATA_COUNT, twelveLogicOverCurrent); + } + return; +} + +void telemetry() +{ + RoveComm.write(RC_POWERBOARD_MOTORBUSCURRENT_DATA_ID, RC_POWERBOARD_MOTORBUSCURRENT_DATA_COUNT, MOTORBUSCURRENTS); + delay(100); + RoveComm.write(RC_POWERBOARD_TWELVEVACTBUSCURRENT_DATA_ID, RC_POWERBOARD_TWELVEVACTBUSCURRENT_DATA_COUNT, AUX_CURRENT); + delay(100); + RoveComm.write(RC_POWERBOARD_TWELVEVLOGICBUSCURRENT_DATA_ID, RC_POWERBOARD_TWELVEVLOGICBUSCURRENT_DATA_COUNT, TWELVELOGICBUSCURRENTS); + delay(100); + Telemetry.begin(telemetry, 150000); } \ No newline at end of file From 54f60362b6eba52aa6b877f37b098b6b43a760fb Mon Sep 17 00:00:00 2001 From: antrob25 Date: Tue, 26 Apr 2022 15:15:34 -0500 Subject: [PATCH 12/14] Added Poe enable --- powerboardcode/powerboard/powerboard.h | 2 +- powerboardcode/powerboard/powerboard.ino | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/powerboardcode/powerboard/powerboard.h b/powerboardcode/powerboard/powerboard.h index bd2bb6d..92fb1bf 100644 --- a/powerboardcode/powerboard/powerboard.h +++ b/powerboardcode/powerboard/powerboard.h @@ -92,7 +92,7 @@ uint8_t twelveLogicOverCurrent = 0; #define MOTOR6_CTL 30 #define MOTORS_CTL 36 -#define MOTOR_DELAY 2000 +#define MOTOR_DELAY 750 //uint8_t currentSense12V[] = {MULTIMEDIA_SENSE, NAV_SENSE, GIMBAL_ACT_SENSE, DRIVE_SENSE, SCISENSOR_ACT_SENSE, NETSWITCH_SENSE, CAM1_SENSE, CAM2_SENSE, BBB_SENSE, AUX_LOG_SENSE}; uint8_t twelveVoltBusses[NUM_12V_PORTS] = {GIMBAL_CTL, DRIVE_CTL, MULTIMEDIA_CTL, NAV_CTL, CAM1_CTL, CAM2_CTL, BBB_CTL, SPARE_CTL}; diff --git a/powerboardcode/powerboard/powerboard.ino b/powerboardcode/powerboard/powerboard.ino index fee31b1..a567816 100644 --- a/powerboardcode/powerboard/powerboard.ino +++ b/powerboardcode/powerboard/powerboard.ino @@ -90,6 +90,7 @@ void setPins() { // sets Spare to OUTPUT pinMode(PACK_SPARE_CTL, OUTPUT); + pinMode(POE_CTL, OUTPUT); // sets motor busses to OUTPUT for (int i = 0 ; i < NUM_MOTORS ; i++) @@ -109,6 +110,8 @@ void setPins() void setPinStates() { + digitalWrite(POE_CTL, HIGH); + digitalWrite(PACK_SPARE_CTL, HIGH); // turns on 12 volt busses for (int i = 0 ; i < NUM_12V_PORTS ; i++) { @@ -116,7 +119,7 @@ void setPinStates() } // turns on Aux bus - digitalWrite(AUX_CTL, HIGH); + digitalWrite(AUX_CTL, LOW); // turns on motor busses after a delay for (int i = 0 ; i < NUM_MOTORS ; i++) From 3641ba1596f0e46f2e40b3101e3e1e4b6e796b39 Mon Sep 17 00:00:00 2001 From: antrob25 Date: Tue, 26 Apr 2022 15:32:53 -0500 Subject: [PATCH 13/14] bruh --- powerboardcode/powerboard/powerboard.h | 2 +- powerboardcode/powerboard/powerboard.ino | 16 +++++++--------- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/powerboardcode/powerboard/powerboard.h b/powerboardcode/powerboard/powerboard.h index 92fb1bf..66b96e2 100644 --- a/powerboardcode/powerboard/powerboard.h +++ b/powerboardcode/powerboard/powerboard.h @@ -92,7 +92,7 @@ uint8_t twelveLogicOverCurrent = 0; #define MOTOR6_CTL 30 #define MOTORS_CTL 36 -#define MOTOR_DELAY 750 +#define MOTOR_DELAY 500 //uint8_t currentSense12V[] = {MULTIMEDIA_SENSE, NAV_SENSE, GIMBAL_ACT_SENSE, DRIVE_SENSE, SCISENSOR_ACT_SENSE, NETSWITCH_SENSE, CAM1_SENSE, CAM2_SENSE, BBB_SENSE, AUX_LOG_SENSE}; uint8_t twelveVoltBusses[NUM_12V_PORTS] = {GIMBAL_CTL, DRIVE_CTL, MULTIMEDIA_CTL, NAV_CTL, CAM1_CTL, CAM2_CTL, BBB_CTL, SPARE_CTL}; diff --git a/powerboardcode/powerboard/powerboard.ino b/powerboardcode/powerboard/powerboard.ino index a567816..26bd099 100644 --- a/powerboardcode/powerboard/powerboard.ino +++ b/powerboardcode/powerboard/powerboard.ino @@ -2,11 +2,10 @@ void setup() { Serial.begin(115200); - Telemetry.begin(telemetry, 150000); setPins(); setPinStates(); - Serial.begin(115200); RoveComm.begin(RC_POWERBOARD_FOURTHOCTET, &TCPServer, RC_ROVECOMM_POWERBOARD_MAC); + Telemetry.begin(telemetry, 1500000); } void loop() @@ -88,16 +87,15 @@ void loop() void setPins() { - // sets Spare to OUTPUT - pinMode(PACK_SPARE_CTL, OUTPUT); - pinMode(POE_CTL, OUTPUT); - // sets motor busses to OUTPUT for (int i = 0 ; i < NUM_MOTORS ; i++) { pinMode(motorBusses[i], OUTPUT); + digitalWrite(motorBusses[i], LOW); } + pinMode(PACK_SPARE_CTL, OUTPUT); + pinMode(POE_CTL, OUTPUT); // sets Aux to OUTPUT pinMode(AUX_CTL, OUTPUT); @@ -110,8 +108,6 @@ void setPins() void setPinStates() { - digitalWrite(POE_CTL, HIGH); - digitalWrite(PACK_SPARE_CTL, HIGH); // turns on 12 volt busses for (int i = 0 ; i < NUM_12V_PORTS ; i++) { @@ -127,6 +123,9 @@ void setPinStates() delay(MOTOR_DELAY); digitalWrite(motorBusses[i], HIGH); } + digitalWrite(PACK_SPARE_CTL, HIGH); + digitalWrite(POE_CTL, HIGH); + } float senseCurrent(const uint8_t sensePin) @@ -203,5 +202,4 @@ void telemetry() delay(100); RoveComm.write(RC_POWERBOARD_TWELVEVLOGICBUSCURRENT_DATA_ID, RC_POWERBOARD_TWELVEVLOGICBUSCURRENT_DATA_COUNT, TWELVELOGICBUSCURRENTS); delay(100); - Telemetry.begin(telemetry, 150000); } \ No newline at end of file From 6ad5473a33b5114d1216482908d015529deba083 Mon Sep 17 00:00:00 2001 From: Grant Brinker Date: Tue, 26 Apr 2022 15:36:55 -0500 Subject: [PATCH 14/14] Added control pin toggling for overcurrent --- powerboardcode/powerboard/powerboard.h | 12 ++++++------ powerboardcode/powerboard/powerboard.ino | 15 ++++++++++++--- 2 files changed, 18 insertions(+), 9 deletions(-) diff --git a/powerboardcode/powerboard/powerboard.h b/powerboardcode/powerboard/powerboard.h index 92fb1bf..84c17ad 100644 --- a/powerboardcode/powerboard/powerboard.h +++ b/powerboardcode/powerboard/powerboard.h @@ -23,9 +23,9 @@ IntervalTimer Telemetry; #define OVERCURRENT_PACK 19500 //mA #define OVERCURRENT_12V 4800 //mA #define CURRENT_ADC_MIN 0 -#define CURRENT_ADC_MAX 0 +#define CURRENT_ADC_MAX 4096 #define CURRENT_mA_MIN 0 -#define CURRENT_mA_MAX 0 +#define CURRENT_mA_MAX 20000 //12V CTL #define SPARE_CTL 0 @@ -51,8 +51,8 @@ IntervalTimer Telemetry; // Current sensing variables -float MOTORBUSPINS[7] = {P_MOTOR1_SENSE, P_MOTOR2_SENSE, P_MOTOR3_SENSE, P_MOTOR4_SENSE, P_MOTOR5_SENSE, P_MOTOR6_SENSE, P_MOTOR7_SENSE}; -float TWELVELOGICBUSPINS[8] = {GIMBAL_ACT_SENSE, DRIVE_SENSE, MULTIMEDIA_SENSE, NAV_SENSE, CAM1_SENSE, CAM2_SENSE, BBB_SENSE, SPARE_12V_SENSE}; +float MOTORBUSSENSEPINS[NUM_MOTORS] = {P_MOTOR1_SENSE, P_MOTOR2_SENSE, P_MOTOR3_SENSE, P_MOTOR4_SENSE, P_MOTOR5_SENSE, P_MOTOR6_SENSE, P_MOTOR7_SENSE}; +float TWELVELOGICBUSPINS[NUM_12V_PORTS] = {GIMBAL_ACT_SENSE, DRIVE_SENSE, MULTIMEDIA_SENSE, NAV_SENSE, CAM1_SENSE, CAM2_SENSE, BBB_SENSE, SPARE_12V_SENSE}; float P_MOTOR1_CURRENT = 0; float P_MOTOR2_CURRENT = 0; @@ -73,8 +73,8 @@ float CAM2_CURRENT = 0; float BBB_CURRENT = 0; float SPARE_12V_CURRENT = 0; -float MOTORBUSCURRENTS[7] = {P_MOTOR1_CURRENT, P_MOTOR2_CURRENT, P_MOTOR3_CURRENT, P_MOTOR4_CURRENT, P_MOTOR5_CURRENT, P_MOTOR6_CURRENT, P_MOTOR7_CURRENT}; -float TWELVELOGICBUSCURRENTS[8] = {GIMBAL_ACT_CURRENT, DRIVE_CURRENT, MULTIMEDIA_CURRENT, NAV_CURRENT, CAM1_CURRENT, CAM2_CURRENT, BBB_CURRENT, SPARE_12V_CURRENT}; +float MOTORBUSCURRENTS[NUM_MOTORS] = {P_MOTOR1_CURRENT, P_MOTOR2_CURRENT, P_MOTOR3_CURRENT, P_MOTOR4_CURRENT, P_MOTOR5_CURRENT, P_MOTOR6_CURRENT, P_MOTOR7_CURRENT}; +float TWELVELOGICBUSCURRENTS[NUM_12V_PORTS] = {GIMBAL_ACT_CURRENT, DRIVE_CURRENT, MULTIMEDIA_CURRENT, NAV_CURRENT, CAM1_CURRENT, CAM2_CURRENT, BBB_CURRENT, SPARE_12V_CURRENT}; uint8_t motorOverCurrent = 0; uint8_t twelveActOverCurrent = 0; diff --git a/powerboardcode/powerboard/powerboard.ino b/powerboardcode/powerboard/powerboard.ino index a567816..744168c 100644 --- a/powerboardcode/powerboard/powerboard.ino +++ b/powerboardcode/powerboard/powerboard.ino @@ -2,7 +2,7 @@ void setup() { Serial.begin(115200); - Telemetry.begin(telemetry, 150000); + Telemetry.begin(telemetry, 1500000); setPins(); setPinStates(); Serial.begin(115200); @@ -140,11 +140,14 @@ void measureCurrent() { for (uint8_t i = 0; i < 7; i++) { - MOTORBUSCURRENTS[i] = senseCurrent(MOTORBUSPINS[i]); + MOTORBUSCURRENTS[i] = senseCurrent(MOTORBUSSENSEPINS[i]); if (MOTORBUSCURRENTS[i] > OVERCURRENT_PACK) { motorOverCurrent |= (1 << i); + digitalWrite(motorBusses[i], LOW); + delay(1000); + digitalWrite(motorBusses[i], HIGH); } else { @@ -156,6 +159,9 @@ void measureCurrent() if (AUX_CURRENT > OVERCURRENT_12V) { twelveActOverCurrent = 1; + digitalWrite(AUX_CTL, LOW); + delay(1000); + digitalWrite(AUX_CTL, HIGH); } else { @@ -169,6 +175,9 @@ void measureCurrent() if (TWELVELOGICBUSCURRENTS[i] > OVERCURRENT_12V) { twelveLogicOverCurrent |= (1 << i); + digitalWrite(twelveVoltBusses[i], LOW); + delay(1000); + digitalWrite(twelveVoltBusses[i], HIGH); } else { @@ -203,5 +212,5 @@ void telemetry() delay(100); RoveComm.write(RC_POWERBOARD_TWELVEVLOGICBUSCURRENT_DATA_ID, RC_POWERBOARD_TWELVEVLOGICBUSCURRENT_DATA_COUNT, TWELVELOGICBUSCURRENTS); delay(100); - Telemetry.begin(telemetry, 150000); + Telemetry.begin(telemetry, 1500000); } \ No newline at end of file