Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions include/CCUTasks.h
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,8 @@ HT_TASK::TaskResponse calculate_charge_current(const unsigned long& sysMicros, c

HT_TASK::TaskResponse print_data(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo);

HT_TASK::TaskResponse update_encoder(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo);



#endif
43 changes: 43 additions & 0 deletions lib/interfaces/include/RotaryEncoderInterface.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
#ifndef ROTARYENCODERINTERFACE_H
#define ROTARYENCODERINTERFACE_H

#include <Arduino.h>
#include <Encoder.h>
#include "CCUData.h"

#include "etl/singleton.h"
#include <etl/delegate.h>

#define CLK 21
#define DT 20
#define SW 19

struct rotary_encoder_s {
volatile bool state = false;
volatile float encoder_value = 0;
unsigned long last_button_press = 0;
const float max_value = 120;
const float min_value = 0;
};

class RotaryEncoderInterface
{
public:
RotaryEncoderInterface(CCUData &ccu_data) :
_ccu_data(ccu_data) {Serial.begin(9600);};

void setupEncoder();
void updateEncoder();
static void isr1();
void set_enc_value(int dt_value);

bool isButtonPressed() const { return _encoder_data.state; };

private:
rotary_encoder_s _encoder_data;
CCUData &_ccu_data;
};

using RotaryEncoderInterfaceInstance = etl::singleton<RotaryEncoderInterface>;

#endif
3 changes: 3 additions & 0 deletions lib/interfaces/src/DisplayInterface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,9 @@ void DisplayInterface::display_data() {
if (_ccu_data.charging_state == ChargingState_e::CHARGING) {
Display.print("Charging at ");
Display.println(_ccu_data.calculated_charge_current);
} else if (_ccu_data.charging_state == ChargingState_e::MANUAL_CHARGING) {
Display.print("Charging (M) at ");
Display.println(_ccu_data.calculated_charge_current);
} else if (_ccu_data.charging_state == ChargingState_e::DONE_CHARGING) {
Display.println("Done charging!");
} else {
Expand Down
58 changes: 58 additions & 0 deletions lib/interfaces/src/RotaryEncoderInterface.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
#include "RotaryEncoderInterface.h"
#include "CCUData.h"


const int bounce_delay = 200; //Worked best during testing to minimize switch bouncing
volatile unsigned long last_interrupt_time = 0;

void RotaryEncoderInterface::isr1() {
unsigned long current_time = millis();
if (current_time - last_interrupt_time > bounce_delay) {
int enc_b_value = digitalRead(DT);
RotaryEncoderInterfaceInstance::instance().set_enc_value(enc_b_value);
last_interrupt_time = current_time;
}

}

void RotaryEncoderInterface::set_enc_value(int enc_b_value) {
if (enc_b_value == HIGH) {
if (_encoder_data.encoder_value < _encoder_data.max_value) {
_encoder_data.encoder_value += 1.0;
}
} else {
if (_encoder_data.encoder_value > _encoder_data.min_value) {
_encoder_data.encoder_value -= 1.0;
}
}
}

void RotaryEncoderInterface::setupEncoder() {
_encoder_data.encoder_value = 0;
_encoder_data.state = false;
pinMode(CLK, INPUT_PULLUP);
pinMode(DT, INPUT_PULLUP);
pinMode(SW, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(CLK), RotaryEncoderInterface::isr1, FALLING);
}

void RotaryEncoderInterface::updateEncoder() {
static float prev_value = -1;
int btn_state = digitalRead(SW);

_ccu_data.encoder_value = _encoder_data.encoder_value;

if (_encoder_data.encoder_value != prev_value) {
Serial.print("Current value: ");
Serial.println(_encoder_data.encoder_value);
}
prev_value = _encoder_data.encoder_value;

if (btn_state == LOW) {
if (millis() - _encoder_data.last_button_press > bounce_delay) {
_encoder_data.state = !_encoder_data.state;
Serial.println("Button pressed!");
}
_encoder_data.last_button_press = millis();
}
}
59 changes: 59 additions & 0 deletions lib/mock_interfaces/RotaryEncoderInterface.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
#ifndef ROTARYENCODERINTERFACE_H
#define ROTARYENCODERINTERFACE_H

#include "CCUData.h"
#include "mockArduino.h"

#include "etl/singleton.h"
#include <etl/delegate.h>

#define CLK 2 //change based on actual pin used
#define DT 3 //change based on actual pin used
#define SW 4 //change based on actual pin used

struct rotary_encoder_s {
volatile bool state = false;
volatile float encoder_value = 0;
unsigned long last_button_press = 0;
const float max_value = 120;
const float min_value = 0;
};

class RotaryEncoderInterface
{
public:
RotaryEncoderInterface(CCUData &ccu_data) :
_ccu_data(ccu_data) {};

inline void setupEncoder() {
_encoder_data.encoder_value = 0;
_encoder_data.state = false;
}

inline void updateEncoder() {
// Sync internal encoder value to CCUData (matching real implementation)
_ccu_data.encoder_value = _encoder_data.encoder_value;
}

inline void set_enc_value(int dt_value) {
if (dt_value == HIGH) {
if (_encoder_data.encoder_value < _encoder_data.max_value) {
_encoder_data.encoder_value += 1.0f;
}
} else {
if (_encoder_data.encoder_value > _encoder_data.min_value) {
_encoder_data.encoder_value -= 1.0f;
}
}
}

bool isButtonPressed() const { return _encoder_data.state; };

private:
rotary_encoder_s _encoder_data;
CCUData &_ccu_data;
};

using RotaryEncoderInterfaceInstance = etl::singleton<RotaryEncoderInterface>;

#endif
2 changes: 2 additions & 0 deletions lib/systems/include/MainChargeSystem.h
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,8 @@

#include "ACUInterface.h"

#include "RotaryEncoderInterface.h"

#include "SharedFirmwareTypes.h"

#ifdef TEENSY_OPT_SMALLEST_CODE
Expand Down
10 changes: 9 additions & 1 deletion lib/systems/src/MainChargeSystem.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@ void MainChargeSystem::calculate_charge_current() {
* If acu_state = 2, we should/are safe to be charging
*/
bool acu_shutdown_low = ACUInterfaceInstance::instance().get_latest_data().acu_state == 1; //NOLINT
bool rotary_state = false;
rotary_state = RotaryEncoderInterfaceInstance::instance().isButtonPressed();

bool voltage_reached = (high_voltage >= _ccu_data.cutoff_voltage) || (ACUInterfaceInstance::instance().get_latest_data().total_voltage > _ccu_data.max_pack_voltage); //NOLINT
if (shutdown_low || acu_shutdown_low)
Expand All @@ -30,7 +32,11 @@ void MainChargeSystem::calculate_charge_current() {
} else if(voltage_reached)
{
_ccu_data.charging_state = ChargingState_e::DONE_CHARGING;
} else {
} else if(rotary_state)
{
_ccu_data.charging_state = ChargingState_e::MANUAL_CHARGING;
}
else {
_ccu_data.charging_state = ChargingState_e::CHARGING;
}

Expand All @@ -39,6 +45,8 @@ void MainChargeSystem::calculate_charge_current() {
if (voltage_reached || shutdown_low || acu_shutdown_low) { //ACU will cause a BMS fault if there is a cell or board temp that is too high
_ccu_data.calculated_charge_current = 0;
_ccu_data.balancing_enabled = false;
} else if(_ccu_data.charging_state == ChargingState_e::MANUAL_CHARGING) {
_ccu_data.calculated_charge_current = _ccu_data.encoder_value;
} else {
_ccu_data.calculated_charge_current = _ccu_data.charger_current_max; // 120 = 3.4 amps
}
Expand Down
8 changes: 7 additions & 1 deletion lib/utilities/CCUData.h
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,8 @@ enum class ChargingState_e
{
NOT_CHARGING = 0,
CHARGING =1,
DONE_CHARGING =2
DONE_CHARGING =2,
MANUAL_CHARGING =3
};

struct CCUData
Expand All @@ -19,6 +20,7 @@ struct CCUData
static constexpr float charger_current_max = 120; //120 = 3.4 amps
static constexpr float safe_charging_current = 15;
float calculated_charge_current = 0;
float encoder_value = 0;
static constexpr float min_pack_voltage = 403; //need to double check this number
static constexpr float max_pack_voltage = 530;
static constexpr const int SHDN_E_READ = 4;
Expand Down Expand Up @@ -59,6 +61,10 @@ namespace CCUConstants
constexpr unsigned long SEND_ALL_DATA_PERIOD = 1000;
constexpr unsigned long HT_SCHED_CAN_PERIOD = 100000;

constexpr unsigned long ROTARY_ENC_PERIOD = 100000;
constexpr unsigned long ROTARY_ENC_PRIORITY = 14;


/* CAN Constants */
const uint32_t CAN_BAUDRATE = 1000000; //CAN for ACU
const uint32_t CHARGER_CAN_BAUDRATE = 500000; //CAN for charger
Expand Down
7 changes: 7 additions & 0 deletions src/CCUTasks.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@ HT_TASK::TaskResponse intitialize_all_interfaces()

EnergyMeterInterfaceInstance::create();

RotaryEncoderInterfaceInstance::create(ccu_data); //NOLINT (necessary for passing ccu_data struct as a reference)


WatchdogInstance::create();
WatchdogInstance::instance().init();
Expand All @@ -31,6 +33,11 @@ HT_TASK::TaskResponse intitialize_all_interfaces()

}

// MOVE IN FUTURE IF NECESSARY
HT_TASK::TaskResponse update_encoder(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) {
RotaryEncoderInterfaceInstance::instance().updateEncoder();
return HT_TASK::TaskResponse::YIELD;
}

HT_TASK::TaskResponse run_read_dial_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo) {

Expand Down
8 changes: 6 additions & 2 deletions src/main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#include "CCUTasks.h"
#include "SystemTimeInterface.h"
#include "DisplayInterface.h"
#include "RotaryEncoderInterface.h"


FlexCAN_Type<CAN2> CHARGER_CAN; //placed here after debugging
Expand All @@ -24,11 +25,12 @@ FlexCAN_Type<CAN1> ACU_CAN;
/* Parameters */
ACUAllData_s acu_all_data;


/* Systems */
namespace qn = qindesign::network; //setup of qn namespace
qn::EthernetUDP udp; //setup of qn namespace

/* Rotary Encoder Setup */
RotaryEncoderInterface& enc = RotaryEncoderInterfaceInstance::instance();

/* Scheduler Setup */
HT_SCHED::Scheduler& scheduler = HT_SCHED::Scheduler::getInstance();
Expand All @@ -49,7 +51,7 @@ HT_TASK::Task kick_watchdog_task(init_kick_watchdog, run_kick_watchdog, CCUConst
HT_TASK::Task debug_print_task(HT_TASK::DUMMY_FUNCTION, print_data, CCUConstants::UPDATE_DISPLAY_PRIORITY, CCUConstants::UPDATE_DISPLAY_PERIOD);
HT_TASK::Task tick_state_machine_task(HT_TASK::DUMMY_FUNCTION, tick_state_machine, CCUConstants::TICK_STATE_MACHINE_PRIORITY, CCUConstants::TICK_STATE_MACHINE_PERIOD);
HT_TASK::Task calculate_charge_current_task(HT_TASK::DUMMY_FUNCTION, calculate_charge_current, CCUConstants::TICK_STATE_MACHINE_PRIORITY, CCUConstants::TICK_STATE_MACHINE_PERIOD);

HT_TASK::Task update_encoder_task(HT_TASK::DUMMY_FUNCTION, update_encoder, CCUConstants::ROTARY_ENC_PRIORITY, CCUConstants::ROTARY_ENC_PERIOD);


void setup() {
Expand All @@ -73,9 +75,11 @@ void setup() {
//scheduler.schedule(tick_state_machine_task); //this task times out watchdog for some reason (state machine would be nice to have but isn't a priority for CCU to work)
scheduler.schedule(calculate_charge_current_task);
scheduler.schedule(update_display_task);
scheduler.schedule(update_encoder_task);

handle_CAN_setup(ACU_CAN, CCUConstants::CAN_BAUDRATE, &CCUCANInterfaceImpl::on_acu_can_receive);
handle_CAN_setup(CHARGER_CAN, CCUConstants::CHARGER_CAN_BAUDRATE, &CCUCANInterfaceImpl::on_charger_can_receive);
enc.setupEncoder();
}


Expand Down
1 change: 1 addition & 0 deletions test/test_charge_cycle.h
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@

#include "MainChargeSystem.h"
#include "ACUInterface.h"
#include "RotaryEncoderInterface.h"
#include "ChargerInterface.h"
#include <gtest/gtest.h>
#include <iostream>
Expand Down
2 changes: 1 addition & 1 deletion test/test_maincharge.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,4 +11,4 @@ int main(int argc, char **argv)
// Do nothing (always return 0 and allow PlatformIO to parse result)
}
return 0;
}
}