diff --git a/Playground/can_speed_test/can_speed_test.ino b/Playground/can_speed_test/can_speed_test.ino new file mode 100644 index 00000000..9b609f28 --- /dev/null +++ b/Playground/can_speed_test/can_speed_test.ino @@ -0,0 +1,51 @@ +#include + +class CAN_msg { +public: + inline void do_nothing(bool val) { this-> vals = val; } + inline void do_with_flags(bool val) { vals = (vals & 0xEF) | (val << 3);} + inline void do_with_ifs(bool val) { (val) ? vals |= (1 << 3) : vals &= ~(1 << 3);} +private: + uint8_t vals; +}; + +void setup() { + Serial.begin(115200); + + CAN_msg msg{}; + srand(0); + int start_millis = millis(); + for (int i = 0; i < 1e9; i++){ + bool val = rand() % 2; + msg.do_nothing(val); + } + int end_millis = millis(); + Serial.print("Do nothing: "); + Serial.println(end_millis - start_millis); + + srand(0); + start_millis = millis(); + for (int i = 0; i < 1e9; i++){ + bool val = rand() % 2; + msg.do_with_flags(val); + } + end_millis = millis(); + Serial.print("Do with flags: "); + Serial.println(end_millis - start_millis); + + srand(0); + start_millis = millis(); + for (int i = 0; i < 1e9; i++){ + bool val = rand() % 2; + msg.do_with_ifs(val); + } + end_millis = millis(); + Serial.print("Do with if block: "); + Serial.println(end_millis - start_millis); + +} + +void loop() { + // put your main code here, to run repeatedly: + +} diff --git a/Playground/pragma_pack/pargma_pack.cc b/Playground/pragma_pack/pargma_pack.cc new file mode 100644 index 00000000..71074329 --- /dev/null +++ b/Playground/pragma_pack/pargma_pack.cc @@ -0,0 +1,32 @@ +#include +#include + +using namespace std; + +typedef struct CAN_message_mcu_status_t { + uint8_t state; + uint8_t flags; + int16_t temperature; + uint16_t glv_battery_voltage; +} no_pack; + +#pragma pack(push, 128) + +typedef struct CAN_message_mcu_status_t2 { + uint8_t state; + uint8_t flags; + int16_t temperature; + uint16_t glv_battery_voltage; +} pack; + +#pragma pack(pop) + +int main(){ + no_pack none; + pack packed; + + printf("%d | %p %p %p %p %p \n", sizeof(none), &none, &none.state, &none.flags, &none.temperature, &none.glv_battery_voltage); + + printf("%d | %p %p %p %p %p \n", sizeof(packed), &packed, &packed.state, &packed.flags, &packed.temperature, &packed.glv_battery_voltage); + +} \ No newline at end of file diff --git a/Playground/pragma_pack/pragma/pragma.ino b/Playground/pragma_pack/pragma/pragma.ino new file mode 100644 index 00000000..5cd33238 --- /dev/null +++ b/Playground/pragma_pack/pragma/pragma.ino @@ -0,0 +1,34 @@ +#include + +typedef struct CAN_message_mcu_status_t { + uint8_t state; + int16_t temperature; + uint16_t glv_battery_voltage; + uint8_t flags; +} no_pack; + +#pragma pack(push, 1) + +typedef struct CAN_message_mcu_status_t2 { + uint8_t state; + int16_t temperature; + uint16_t glv_battery_voltage; + uint8_t flags; +} pack; + +#pragma pack(pop) + +void setup(){ + Serial.begin(115200); + no_pack none; + pack packed; + + Serial.printf("%d | %p %p %p %p %p \n", sizeof(none), &none, &none.state, &none.flags, &none.temperature, &none.glv_battery_voltage); + + Serial.printf("%d | %p %p %p %p %p \n", sizeof(packed), &packed, &packed.state, &packed.flags, &packed.temperature, &packed.glv_battery_voltage); + +} + +void loop(){ + +} diff --git a/Playground/pragma_pack/pragma_class.cc b/Playground/pragma_pack/pragma_class.cc new file mode 100644 index 00000000..a3fd35bc --- /dev/null +++ b/Playground/pragma_pack/pragma_class.cc @@ -0,0 +1,57 @@ +#include +#include +#include +#include + +using namespace std; + + +#pragma pack(push,1) + +class MCU_wheel_speed { +public: + MCU_wheel_speed() = default; + + MCU_wheel_speed(const uint8_t buf[8]) { load(buf); } + + inline void load(const uint8_t buf[8]) { + memcpy(&wheel_rpm_front_left, &buf[0], sizeof(wheel_rpm_front_left)); + memcpy(&wheel_rpm_front_right, &buf[2], sizeof(wheel_rpm_front_right)); + memcpy(&wheel_rpm_rear_left, &buf[4], sizeof(wheel_rpm_rear_left)); + memcpy(&wheel_rpm_rear_right, &buf[6], sizeof(wheel_rpm_rear_right)); + } + + inline void write(uint8_t buf[8]) const { + memcpy(&buf[0], &wheel_rpm_front_left, sizeof(wheel_rpm_front_left)); + memcpy(&buf[2], &wheel_rpm_front_right, sizeof(wheel_rpm_front_right)); + memcpy(&buf[4], &wheel_rpm_rear_left, sizeof(wheel_rpm_rear_left)); + memcpy(&buf[6], &wheel_rpm_rear_right, sizeof(wheel_rpm_rear_right)); + } + + // Getters + inline uint16_t getRpmFrontLeft() const { return wheel_rpm_front_left; } + inline uint16_t getRpmFrontRight() const { return wheel_rpm_front_right; } + inline uint16_t getRpmRearLeft() const { return wheel_rpm_rear_left; } + inline uint16_t getRpmRearRight() const { return wheel_rpm_rear_right; } + + // Setters + inline void setRpmFrontLeft(uint16_t speed) { wheel_rpm_front_left = speed; } + inline void setRpmFrontRight(uint16_t speed) { wheel_rpm_front_right = speed; } + inline void setRpmRearLeft(uint16_t speed) { wheel_rpm_rear_left = speed; } + inline void setRpmRearRight(uint16_t speed) { wheel_rpm_rear_right = speed; } + +private: + uint16_t wheel_rpm_front_left; + uint16_t wheel_rpm_front_right; + uint16_t wheel_rpm_rear_left; + uint16_t wheel_rpm_rear_right; +}; + +#pragma pack(pop) + +int main(){ + cout << is_pod() << endl; + cout << is_standard_layout() << endl; + cout << is_trivial() << endl; + cout << sizeof(MCU_wheel_speed) << endl; +} \ No newline at end of file diff --git a/Playground/wheel_speed_testing/wheel_speed_testing.ino b/Playground/wheel_speed_testing/wheel_speed_testing.ino new file mode 100644 index 00000000..3af2cf46 --- /dev/null +++ b/Playground/wheel_speed_testing/wheel_speed_testing.ino @@ -0,0 +1,82 @@ +#include + +// print frequency every 100 ms +Metro printer(100); + +#define FRONT_LEFT_WHEEL 10 +#define FRONT_RIGHT_WHEEL 15 +#define TIME_OUT 500000 + +float rpm_front_left{}; +float rpm_front_right{}; + +void setup() { + // put your setup code here, to run once: + pinMode(FRONT_RIGHT_WHEEL, INPUT); + pinMode(FRONT_LEFT_WHEEL, INPUT); + pinMode(13, OUTPUT); + digitalWrite(13, HIGH); + Serial.begin(9600); + Serial.println("Starting up"); +} + +void loop() { + read_wheel_speed(); + if (printer.check()) { + Serial.print("RPM Left: "); + Serial.print(rpm_front_left); + Serial.print(" RPM Right: "); + Serial.print(rpm_front_right); + Serial.println(); + } +} + +void read_wheel_speed() { + static int micros_elapsed{}; + { + static uint8_t cur_state_front_left{}; + static uint8_t prev_state_front_left{}; + static int cur_time_front_left{}; + static int prev_time_front_left{}; + + cur_state_front_left = digitalRead(FRONT_LEFT_WHEEL); + + if (cur_state_front_left == 0 && prev_state_front_left == 1) { + cur_time_front_left = micros(); + micros_elapsed = cur_time_front_left - prev_time_front_left; + if (micros_elapsed > 500) { + rpm_front_left = 1.0 / micros_elapsed; + prev_time_front_left = cur_time_front_left; + } + } + + if (micros() - prev_time_front_left > TIME_OUT && rpm_front_left) { + rpm_front_left = 0; + } + + prev_state_front_left = cur_state_front_left; + } + { + static uint8_t cur_state_front_right{}; + static uint8_t prev_state_front_right{}; + static int cur_time_front_right{}; + static int prev_time_front_right{}; + + cur_state_front_right = digitalRead(FRONT_RIGHT_WHEEL); + + if (cur_state_front_right == 0 && prev_state_front_right == 1) { + cur_time_front_right = micros(); + micros_elapsed = cur_time_front_right - prev_time_front_right; + if (micros_elapsed > 500) { + rpm_front_right = 1.0 / micros_elapsed; + prev_time_front_right = cur_time_front_right; + } + } + + if (micros() - prev_time_front_right > TIME_OUT && rpm_front_right) { + rpm_front_right = 0; + } + + prev_state_front_right = cur_state_front_right; + } +}