Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Initial test technique for CAN messages -- Random messages #7

Open
wants to merge 7 commits into
base: main
Choose a base branch
from
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
10 changes: 5 additions & 5 deletions include/IOManagement.h
Original file line number Diff line number Diff line change
Expand Up @@ -8,13 +8,13 @@
#include <Arduino.h>

//Outputs
#define MCU_DIR PA_6
#define MCU_ECO PB_1
#define MCU_MC_ON PA_2
#define MCU_DIR PA6
#define MCU_ECO PB1
#define MCU_MC_ON PA2

//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
Expand Down
3 changes: 2 additions & 1 deletion include/const.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
3 changes: 0 additions & 3 deletions include/motor_control.h
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,4 @@ PDCStates get_state();
extern volatile float rpm;
extern volatile float motorSpeedSetpoint;

// cruise control variables
PID *curr_PID;

#endif
2 changes: 2 additions & 0 deletions src/canPDC.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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*)&regen_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*)&current_in_telem, sizeof(float));
Expand Down
78 changes: 76 additions & 2 deletions src/main.cpp
Original file line number Diff line number Diff line change
@@ -1,16 +1,90 @@
#include <Arduino.h>
#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 <stdlib.h>
#include <time.h>

#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);

initIO();
initPDCState();

//initialize random
srand(time(NULL));
randomizeData();
}

void loop() {
// Display digital and analog values every second (for testing)
#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);
}
5 changes: 4 additions & 1 deletion src/motor_control.cpp
Original file line number Diff line number Diff line change
@@ -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;

Expand All @@ -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;
Expand Down