diff --git a/include/IOManagement.h b/include/IOManagement.h index 8a749b6..c81ff34 100644 --- a/include/IOManagement.h +++ b/include/IOManagement.h @@ -8,13 +8,13 @@ #include //Outputs -#define MCU_DIR PA_6 -#define MCU_ECO PB_1 -#define MCU_MC_ON PA_2 +#define MCU_DIR LL_PWR_GPIO_BIT_7 +#define MCU_ECO PB1 +#define MCU_MC_ON PA10 //Inputs -#define MCU_SPEED_SIG PA_8 -#define PRK_BRK_TELEM PB_4 +#define MCU_SPEED_SIG PA8 +#define PRK_BRK_TELEM PB4 struct Digital_Data { bool direction : 1; // output diff --git a/include/const.h b/include/const.h index 4e4a606..801c777 100644 --- a/include/const.h +++ b/include/const.h @@ -5,7 +5,8 @@ * Debugging Techniques: * 1. Send random data over CAN and verify if you received the same random data */ -#define DEBUG_TECHNIQUE 1 + +#define COUNTER_EXP 100 #define IO_UPDATE_PERIOD 100000 // us #define PID_UPDATE_INTERVAL IO_UPDATE_PERIOD/1000000 // sec diff --git a/include/motor_control.h b/include/motor_control.h index f4f0c1d..bc7ff91 100644 --- a/include/motor_control.h +++ b/include/motor_control.h @@ -16,7 +16,4 @@ PDCStates get_state(); extern volatile float rpm; extern volatile float motorSpeedSetpoint; -// cruise control variables -PID *curr_PID; - #endif \ No newline at end of file diff --git a/src/canPDC.cpp b/src/canPDC.cpp index dda854e..09c49d0 100644 --- a/src/canPDC.cpp +++ b/src/canPDC.cpp @@ -17,9 +17,11 @@ void CANPDC::readHandler(CAN_message_t msg) { } void CANPDC::sendPDCData() { + // Serial.printf("message sent: %d\n",this->sendMessage(0x200, (void*)&acc_out, sizeof(float))); this->sendMessage(0x200, (void*)&acc_out, sizeof(float)); this->sendMessage(0x201, (void*)®en_brake, sizeof(float)); this->sendMessage(0x202, (void*)&lv_12V_telem, sizeof(float)); + // Serial.printf("sent or not %f",this->sendMessage(0x203, (void*)&lv_5V_telem, sizeof(float))); this->sendMessage(0x203, (void*)&lv_5V_telem, sizeof(float)); this->sendMessage(0x204, (void*)&lv_5V_current, sizeof(float)); this->sendMessage(0x205, (void*)¤t_in_telem, sizeof(float)); diff --git a/src/main.cpp b/src/main.cpp index 4bf9ce6..0d06a58 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,16 +1,102 @@ #include -#include "IOManagement.h" -#include "const.h" + #include "canPDC.h" +#include "const.h" +#include "IOManagement.h" +#include "motor_control.h" +#include "adc.h" + +//For random +#include +#include + +#define DEBUG_TECHNIQUE 1 + +int counter = 0; + +//Counter for debug prints 100 ticks equates to 5 seconds CANPDC canBus(CAN1, DEF); + +#if DEBUG_TECHNIQUE == 1 +//Define methods +void randomizeData(); +void debugPrint(); + +void randomizeData() { + // Generate a random float between 0 and 100 + acc_out = ((float)rand() / RAND_MAX) * 100; + regen_brake = ((float)rand() / RAND_MAX) * 100; + lv_12V_telem = ((float)rand() / RAND_MAX) * 100; + lv_5V_telem = ((float)rand() / RAND_MAX) * 100; + lv_5V_current = ((float)rand() / RAND_MAX) * 100; + current_in_telem = ((float)rand() / RAND_MAX) * 100; + brake_pressure_telem = ((float)rand() / RAND_MAX) * 100; + + //Random boolean + brakeLED = rand() % 2; + + //Random digital data + digital_data.direction = rand() % 2; + digital_data.mc_speed_sig = rand() % 2; + digital_data.eco_mode = rand() % 2; + digital_data.mcu_mc_on = rand() % 2; + digital_data.park_brake = rand() % 2; +} +void debugPrint() { + Serial.printf("acc_out: %f\n", acc_out); + Serial.printf("regen_brake: %f\n", regen_brake); + Serial.printf("lv_12V_telem: %f\n", lv_12V_telem); + Serial.printf("lv_5V_telem: %f\n", lv_5V_telem); + Serial.printf("lv_5V_current: %f\n", lv_5V_current); + Serial.printf("current_in_telem: %f\n", current_in_telem); + Serial.printf("brake_pressure_telem: %f\n", brake_pressure_telem); + Serial.printf("brakeLED: %i\n", brakeLED); + Serial.printf("digital_data.direction: %i\n", digital_data.direction); + Serial.printf("digital_data.mc_speed_sig: %i\n", digital_data.mc_speed_sig); + Serial.printf("digital_data.eco_mode: %i\n", digital_data.eco_mode); + Serial.printf("digital_data.mcu_mc_on: %i\n", digital_data.mcu_mc_on); + Serial.printf("digital_data.park_brake: %i\n", digital_data.park_brake); +} +#endif + + void setup() { Serial.begin(115200); + + Serial.printf("Starting up...\n"); + initIO(); + initPDCState(); + + //initialize random + srand(time(NULL)); + randomizeData(); + pinMode(PB6, OUTPUT); } void loop() { + // Display digital and analog values every second (for testing) + + digitalWrite(PB6, HIGH); + + delay(500); + + digitalWrite(PB6, LOW); + + delay(500); + + #if DEBUG_TECHNIQUE == 1 + if (counter >= COUNTER_EXP) { + randomizeData(); + debugPrint(); + counter = 0; + //Receiving + Serial.printf("Received forwardANdReverse: %i\n", forwardAndReverse); + } + counter++; + #endif canBus.sendPDCData(); canBus.runQueue(DATA_SEND_PERIOD); } \ No newline at end of file diff --git a/src/motor_control.cpp b/src/motor_control.cpp index eebfb20..b6a77ae 100644 --- a/src/motor_control.cpp +++ b/src/motor_control.cpp @@ -1,6 +1,9 @@ #include "motor_control.h" +// cruise control variables +PID *curr_PID; + volatile PDCStates pdcState = PDCStates::OFF; volatile CRUZ_MODE cruzMode = CRUZ_MODE::OFF; @@ -9,7 +12,7 @@ PID power_PID(POWER_D_PARAM, POWER_I_PARAM, POWER_P_PARAM, PID_UPDATE_INTERVAL); PID speed_PID(SPEED_D_PARAM, SPEED_I_PARAM, SPEED_P_PARAM, PID_UPDATE_INTERVAL); volatile float speed_pid_compute = 0.0; -STM32TimerInterrupt state_updater(TIM7); +STM32TimerInterrupt state_updater(TIM2); volatile float rpm; volatile float motorSpeedSetpoint;