Weeb Wagon
+Danger Zone
@@ -56,7 +56,6 @@General
State:
Mode:
Mobility:
-ESTOP:
@@ -85,14 +84,14 @@
- Is Fixed:
Satellites:
+
+ Path Planning
@@ -105,6 +104,25 @@Distance to Waypoint:
Waypoints:
+
+
+ Vision
+
+
+
+ ![]()
+ ![]()
+
+
+ ![]()
+ ![]()
+
+
+ ![]()
+ ![]()
+
+ Device States
@@ -117,18 +135,37 @@Device States
-
- ${preset} `);
+ presetElement.append(dropdownItem);
+
+ dropdownItem.on("click", function () {
+ const preset_name = $(this).children().attr("data-value");
+ send({
+ op: "set_active_preset",
+ preset: preset_name
+ });
+ send({ op: "get_presets" });
+ });
+ }
+
+ current_preset = obj.active_preset;
+ $("#active_preset_value").text(current_preset);
+ }
+
if (op == "get_nodes_callback") {
+ console.log(obj);
for (let i = 0; i < obj.nodes.length; i++) {
const node = obj.nodes[i];
send({
@@ -70,13 +100,43 @@ $(document).ready(function () {
opcode: 4,
iterator: iterate()
});
+
+ const statemap = obj.states;
+ if (node in statemap) {
+ if (node == "rosbridge_websocket" || node == "rosapi" || node == "scr_core" || node == "rosapi_params") {
+ continue;
+ }
+
+ deviceStates[node] = statemap[node];
+ unorderedListElement = $("#element_device_states");
+ unorderedListElement.empty();
+ for (const id in deviceStates) {
+ const state = deviceStates[id];
+ unorderedListElement.append(``);
- const deviceBody = $(``);
- deviceElement.append(deviceBody);
-
- for (const address of Object.keys(deviceConfig).sort()) {
- const data = deviceConfig[address];
- const type = addressKeys[deviceId][address];
- if (type == undefined) {
- const alert = $(``);
+ const deviceBody = $(``);
+ deviceElement.append(deviceBody);
+
+ const deviceConfig = config[deviceId];
+ for (const address in addressKeys[deviceId]) {
+ if (address == "internal_title") {
+ continue;
+ }
+
+ if (!(address in deviceConfig)) {
+ const alert = $(``);
+ // const deviceBody = $(``);
+ // deviceElement.append(deviceBody);
+
+ // for (const address of Object.keys(deviceConfig).sort()) {
+ // const data = deviceConfig[address];
+ // const type = addressKeys[deviceId][address];
+ // if (type == undefined) {
+ // const alert = $(`
+#include
+#include
+
+#include "CONBus.h"
+
+namespace CONBus{
+
+typedef struct{
+ uint8_t registerAddress;
+} CAN_readRegisterMessage;
+
+typedef struct{
+ uint8_t registerAddress;
+ uint8_t length;
+ uint8_t reserved_;
+ uint8_t value[4];
+} CAN_readRegisterResponseMessage;
+
+typedef struct{
+ uint8_t registerAddress;
+ uint8_t length;
+ uint8_t reserved_;
+ uint8_t value[4];
+} CAN_writeRegisterMessage;
+
+typedef struct{
+ uint8_t registerAddress;
+ uint8_t length;
+ uint8_t reserved_;
+ uint8_t value[4];
+} CAN_writeRegisterResponseMessage;
+
+class CANBusDriver {
+ public:
+ CANBusDriver(CONBus& conbus, const uint32_t device_id);
+
+ uint8_t readCanMessage(const uint32_t can_id, const void* buffer);
+ bool isReplyReady();
+ uint8_t peekReply(uint32_t& can_id, uint8_t& can_len, void* buffer);
+ uint8_t popReply();
+
+ private:
+ CONBus& conbus_;
+ const uint8_t device_id_;
+
+ CAN_readRegisterMessage readRegisterMessage_;
+ CAN_readRegisterResponseMessage readRegisterResponseMessage_;
+ CAN_writeRegisterMessage writeRegisterMessage_;
+ CAN_writeRegisterResponseMessage writeRegisterResponseMessage_;
+
+ bool awaiting_write_response_ = false;
+
+ void putRegisterAddressInQueue(const uint32_t register_address);
+ uint8_t register_fetch_queue_[256];
+ uint8_t register_fetch_queue_head_ = 0; // located at next entry to send
+ uint8_t register_fetch_queue_tail_ = 0; // located *after* the last entry in the queue
+};
+
+inline CANBusDriver::CANBusDriver(CONBus& conbus, const uint32_t device_id) : conbus_(conbus), device_id_(device_id) {}
+
+inline uint8_t CANBusDriver::readCanMessage(const uint32_t can_id, const void* buffer) {
+ // CONBus read register
+ if (can_id == ((uint32_t)1000 + device_id_)) {
+ readRegisterMessage_ = *(CAN_readRegisterMessage*)buffer;
+ if (readRegisterMessage_.registerAddress != 0xFF) {
+ putRegisterAddressInQueue(readRegisterMessage_.registerAddress);
+ } else {
+ // Put the whole memory map into the queue
+ for (int i=0; i<255; i++) {
+ if (conbus_.hasRegister(i)) {
+ putRegisterAddressInQueue(i);
+ }
+ }
+ }
+ }
+
+ // CONBus write register
+ if (can_id == ((uint32_t)1200 + device_id_)) {
+ awaiting_write_response_ = true;
+
+ writeRegisterMessage_ = *(CAN_writeRegisterMessage*)buffer;
+ conbus_.writeRegisterBytes(writeRegisterMessage_.registerAddress, writeRegisterMessage_.value, writeRegisterMessage_.length);
+
+ memcpy(&writeRegisterResponseMessage_, &writeRegisterMessage_, sizeof(writeRegisterResponseMessage_));
+ }
+
+ return SUCCESS;
+}
+
+inline bool CANBusDriver::isReplyReady() {
+ return (register_fetch_queue_head_ != register_fetch_queue_tail_) || awaiting_write_response_;
+}
+
+inline uint8_t CANBusDriver::peekReply(uint32_t& can_id, uint8_t& can_len, void* buffer) {
+ if (register_fetch_queue_head_ != register_fetch_queue_tail_) {
+ readRegisterResponseMessage_.registerAddress = register_fetch_queue_[register_fetch_queue_head_];
+ conbus_.readRegisterBytes(readRegisterResponseMessage_.registerAddress, readRegisterResponseMessage_.value, readRegisterResponseMessage_.length);
+
+ can_id = 1100 + device_id_;
+ // The readRegisterResponseMessage_ is the full 7 bytes for a 4 byte buffer
+ // but if we have a smaller message, we should reduce the size
+ can_len = sizeof(readRegisterResponseMessage_) - (4 - readRegisterResponseMessage_.length);
+
+ memcpy(buffer, &readRegisterResponseMessage_, sizeof(readRegisterResponseMessage_));
+
+ return SUCCESS; // end early so we dont overwrite a read response with a write response
+ }
+
+ if (awaiting_write_response_) {
+ can_id = 1300 + device_id_;
+ // Same as above, we have to reduce the size appropriately.
+ can_len = sizeof(writeRegisterResponseMessage_) - (4 - writeRegisterResponseMessage_.length);
+ memcpy(buffer, &writeRegisterResponseMessage_, sizeof(writeRegisterResponseMessage_));
+ }
+
+ return SUCCESS;
+}
+
+inline uint8_t CANBusDriver::popReply() {
+ if (register_fetch_queue_head_ != register_fetch_queue_tail_) {
+ // Move head of the queue
+ register_fetch_queue_head_++;
+
+ return SUCCESS; // end early so we dont overwrite a read response with a write response
+ }
+
+ if (awaiting_write_response_) {
+ awaiting_write_response_ = false;
+ }
+
+ return SUCCESS;
+}
+
+inline void CANBusDriver::putRegisterAddressInQueue(const uint32_t register_address) {
+ register_fetch_queue_[register_fetch_queue_tail_] = register_address;
+ register_fetch_queue_tail_++;
+}
+
+
+} // end CONBus namespace
+
+#endif
\ No newline at end of file
diff --git a/firmware/motor_control/CONBus.h b/firmware/motor_control/CONBus.h
new file mode 100644
index 00000000..941b385e
--- /dev/null
+++ b/firmware/motor_control/CONBus.h
@@ -0,0 +1,165 @@
+#ifndef CONBUS_H
+#define CONBUS_H
+
+#include
+#include
+#include
+
+namespace CONBus{
+
+// Define error codes for all CONBus methods
+static const uint8_t SUCCESS = 0;
+static const uint8_t ERROR_UNKNOWN = 1;
+static const uint8_t ERROR_ADDRESS_ALREADY_USED = 2;
+static const uint8_t ERROR_INVALID_LENGTH = 3;
+static const uint8_t ERROR_ADDRESS_NOT_REGISTERED = 4;
+static const uint8_t ERROR_DIFFERENT_TYPE_THAN_REGISTERED = 5;
+
+// Define CONBus specification constants
+static const uint8_t MAX_REGISTER_LENGTH = 4;
+
+class CONBus {
+ public:
+ CONBus();
+
+ template
+ uint8_t addRegister(const uint8_t conbus_address, T* register_address);
+ template
+ uint8_t addReadOnlyRegister(const uint8_t conbus_address, T* register_address);
+
+ bool hasRegister(const uint8_t conbus_address);
+
+ template
+ uint8_t writeRegister(const uint8_t conbus_address, const T value);
+ uint8_t writeRegisterBytes(const uint8_t conbus_address, const void* buffer, const uint8_t length);
+
+ template
+ uint8_t readRegister(const uint8_t conbus_address, T& value);
+ uint8_t readRegisterBytes(const uint8_t conbus_address, void* buffer, uint8_t& length);
+ private:
+ uint8_t length_map_[256];
+ void* pointer_map_[256];
+ bool read_only_map_[256];
+};
+
+inline CONBus::CONBus() {
+ // Clear Length map
+ for (int i=0; i<256; i++) {
+ length_map_[i] = 0;
+ }
+
+ // Clear Pointer map
+ for (int i=0; i<256; i++) {
+ pointer_map_[i] = nullptr;
+ }
+
+ // Clear Read Only map
+ for (int i=0; i<256; i++) {
+ read_only_map_[i] = false;
+ }
+}
+
+template
+inline uint8_t CONBus::addRegister(const uint8_t conbus_address, T* register_address) {
+ if (length_map_[conbus_address] > 0) {
+ return ERROR_ADDRESS_ALREADY_USED;
+ }
+
+ if (sizeof(T) > MAX_REGISTER_LENGTH) {
+ return ERROR_INVALID_LENGTH;
+ }
+
+ length_map_[conbus_address] = sizeof(T);
+ pointer_map_[conbus_address] = register_address;
+ read_only_map_[conbus_address] = false;
+
+ return SUCCESS;
+}
+
+template
+inline uint8_t CONBus::addReadOnlyRegister(const uint8_t conbus_address, T* register_address) {
+ if (length_map_[conbus_address] > 0) {
+ return ERROR_ADDRESS_ALREADY_USED;
+ }
+
+ if (sizeof(T) > MAX_REGISTER_LENGTH) {
+ return ERROR_INVALID_LENGTH;
+ }
+
+ length_map_[conbus_address] = sizeof(T);
+ pointer_map_[conbus_address] = register_address;
+ read_only_map_[conbus_address] = true;
+
+ return SUCCESS;
+}
+
+inline bool CONBus::hasRegister(const uint8_t conbus_address) {
+ return length_map_[conbus_address] > 0;
+}
+
+template
+inline uint8_t CONBus::writeRegister(const uint8_t conbus_address, const T value) {
+ if (length_map_[conbus_address] == 0) {
+ return ERROR_ADDRESS_NOT_REGISTERED;
+ }
+
+ if (sizeof(T) != length_map_[conbus_address]) {
+ return ERROR_DIFFERENT_TYPE_THAN_REGISTERED;
+ }
+
+ if (read_only_map_[conbus_address]) {
+ // If this register is read only, simply don't write.
+ // TODO: Allow this to be configured to be an ERROR.
+ return SUCCESS;
+ }
+
+ *(T*)(pointer_map_[conbus_address]) = value;
+
+ return SUCCESS;
+}
+
+inline uint8_t CONBus::writeRegisterBytes(const uint8_t conbus_address, const void* buffer, const uint8_t length) {
+ if (length_map_[conbus_address] == 0) {
+ return ERROR_ADDRESS_NOT_REGISTERED;
+ }
+
+ if (length != length_map_[conbus_address]) {
+ return ERROR_DIFFERENT_TYPE_THAN_REGISTERED;
+ }
+
+ if (read_only_map_[conbus_address]) {
+ // If this register is read only, simply don't write.
+ // TODO: Allow this to be configured to be an ERROR.
+ return SUCCESS;
+ }
+
+ memcpy(pointer_map_[conbus_address], buffer, length);
+
+ return SUCCESS;
+}
+
+template
+inline uint8_t CONBus::readRegister(const uint8_t conbus_address, T& value) {
+ if (length_map_[conbus_address] == 0) {
+ return ERROR_ADDRESS_NOT_REGISTERED;
+ }
+
+ if (sizeof(T) != length_map_[conbus_address]) {
+ return ERROR_DIFFERENT_TYPE_THAN_REGISTERED;
+ }
+
+ value = *(T*)(pointer_map_[conbus_address]);
+
+ return SUCCESS;
+}
+
+inline uint8_t CONBus::readRegisterBytes(const uint8_t conbus_address, void* buffer, uint8_t& length) {
+ memcpy(buffer, pointer_map_[conbus_address], length_map_[conbus_address]);
+ length = length_map_[conbus_address];
+
+ return SUCCESS;
+}
+
+} // end CONBus namespace
+
+#endif
\ No newline at end of file
diff --git a/firmware/motor_control/differential_drive.h b/firmware/motor_control/differential_drive.h
index d5a912ab..c4a0e937 100644
--- a/firmware/motor_control/differential_drive.h
+++ b/firmware/motor_control/differential_drive.h
@@ -44,9 +44,9 @@ class DifferentialDrive {
MotorWithEncoder right_motor_;
float update_period_ = 0.025f;
- float pulses_per_radian_ = 600.0f * 20.0f / 16.8f;
- float wheel_radius_ = 0.135f;
- float wheelbase_length_ = 0.45f;
+ float pulses_per_radian_ = 600.0f;
+ float wheel_radius_ = 0.19f;
+ float wheelbase_length_ = 0.575f;
float left_encoder_factor_ = 1.00f;
float right_encoder_factor_ = 1.01f;
diff --git a/firmware/motor_control/motor_control.ino b/firmware/motor_control/motor_control.ino
index 0851951d..fab68d40 100644
--- a/firmware/motor_control/motor_control.ino
+++ b/firmware/motor_control/motor_control.ino
@@ -4,37 +4,44 @@
#include "differential_drive.h"
#include "motor_with_encoder.h"
#include "common.h"
-#include
-#include
-
-//LEDs
-const int ledPin = LED_BUILTIN;
-const int CANLED = 0;
-const int LED0 = 1;
-
-//Encoder Pins
-const int encoderLeftA = 3;
-const int encoderLeftB = 2;
-const int encoderRightA = 5;
-const int encoderRightB = 4;
-//Motor PWM pins
-const int leftMotorPwmPin = 16;
-const int rightMotorPwmPin = 14;
-
-const int estopPin = 27;
-
-//SPI pins
-static const byte MCP2515_SCK = 10; // SCK input of MCP2515
-static const byte MCP2515_MOSI = 11; // SDI input of MCP2515
-static const byte MCP2515_MISO = 8; // SDO output of MCP2515
-static const byte MCP2515_CS = 9; // CS input of MCP2515
-static const byte MCP2515_INT = 7; // INT output of MCP2515
-
-ACAN2515 can(MCP2515_CS, SPI1, MCP2515_INT);
+#include "CONBus.h"
+#include "CANBusDriver.h"
-TickTwo motor_update_timer(setMotorUpdateFlag, 25);
+//LED
+static int LED1 = 22;
+static int LED2 = 21;
+
+//ESTOP
+static int ESTOP = 27;
+
+//PWM
+static int PWM0 = 6; //right motor controller
+static int PWM1 = 7; //left motor controller
+
+//ENCODER
+static int ENC0A = 9; //right encoder
+static int ENC0B = 8; //right encoder
+static int ENC1A = 11; //left encoder
+static int ENC1B = 10; //left encoder
+
+//EEPROM
+static int EEPROMSDA = 0;
+static int EEPROMSCL = 1;
+static int EEPROMWP = 2;
+
+//SPI
+static int MCP2515_MISO = 16;
+static int MCP2515_CS = 17;
+static int MCP2515_SCK = 18;
+static int MCP2515_MOSI = 19;
+static int MCP2515_INT = 20;
+
+//Quartz oscillator - 8MHz
+static uint32_t QUARTZ_FREQUENCY = 8UL * 1000UL * 1000UL;
-static const uint32_t QUARTZ_FREQUENCY = 8UL * 1000UL * 1000UL; // 8 MHz
+ACAN2515 can(MCP2515_CS, SPI, MCP2515_INT);
+
+TickTwo motor_update_timer(setMotorUpdateFlag, 25);
CANMessage frame;
CANMessage outFrame;
@@ -47,11 +54,11 @@ robotStatus_t roboStatus;
distance motorDistances;
MotorCommand motorCommand;
-// motor leftMotor(leftMotorPwmPin, true);
-// motor rightMotor(rightMotorPwmPin, false);
+// motor leftMotor(PWM1, true);
+// motor rightMotor(PWM0, false);
-MotorWithEncoder leftMotor(leftMotorPwmPin, encoderLeftA, encoderLeftB, true);
-MotorWithEncoder rightMotor(rightMotorPwmPin, encoderRightA, encoderRightB, false);
+MotorWithEncoder leftMotor(PWM1, ENC1A, ENC1B, true);
+MotorWithEncoder rightMotor(PWM0, ENC0A, ENC0B, false);
DifferentialDrive drivetrain(leftMotor, rightMotor, 0.025);
void configureCan();
@@ -80,28 +87,37 @@ float desired_forward_velocity;
float desired_angular_velocity;
void setup() {
+ Serial.begin(9600);
+ //while(!Serial){
+ //delay(50);
+ //}
delay(50);
+ Serial.println("Serial Connected");
drivetrain.setup();
- pinMode(encoderRightA, INPUT);
- pinMode(encoderRightB, INPUT);
- pinMode(encoderLeftA, INPUT);
- pinMode(encoderLeftB, INPUT);
+ pinMode(ENC0A, INPUT);
+ pinMode(ENC0B, INPUT);
+ pinMode(ENC1A, INPUT);
+ pinMode(ENC1B, INPUT);
- attachInterrupt(encoderLeftA, updateLeft, CHANGE);
- attachInterrupt(encoderRightA, updateRight, CHANGE);
+ attachInterrupt(ENC1A, updateLeft, CHANGE);
+ attachInterrupt(ENC0A, updateRight, CHANGE);
motor_update_timer.start();
-}
-void setup1(){
- Serial.begin(9600);
+
+ //setup1
delay(50);
configureCan();
- pinMode(LED0, OUTPUT);
- pinMode(CANLED, OUTPUT);
- pinMode(estopPin, INPUT);
+ pinMode(LED1, OUTPUT);
+ pinMode(LED2, OUTPUT);
+ digitalWrite(LED1, HIGH);
+ digitalWrite(LED2, HIGH);
+ delay(1000);
+ digitalWrite(LED1, LOW);
+ digitalWrite(LED2, LOW);
+ pinMode(ESTOP, INPUT);
conbus.addReadOnlyRegister(0x00, drivetrain.getUpdatePeriod());
conbus.addRegister(0x01, drivetrain.getPulsesPerRadian());
@@ -128,7 +144,9 @@ void setup1(){
conbus.addRegister(0x50, &motor_updates_between_deltaodom);
}
+
void loop() {
+
updateTimers();
if (MOTOR_UPDATE_FLAG) {
@@ -137,9 +155,8 @@ void loop() {
MOTOR_UPDATE_FLAG = false;
}
-}
-void loop1(){
+ //loop1
if (motor_updates_in_deltaodom >= motor_updates_between_deltaodom) {
motor_updates_in_deltaodom = 0;
sendCanOdomMsgOut();
@@ -155,24 +172,26 @@ void loop1(){
conbus_can.popReply();
}
}
+
}
-void configureCan() {
- SPI1.setSCK(MCP2515_SCK);
- SPI1.setTX(MCP2515_MOSI);
- SPI1.setRX(MCP2515_MISO);
- SPI1.setCS(MCP2515_CS);
- SPI1.begin();
+void configureCan() {
+ SPI.setSCK(MCP2515_SCK);
+ SPI.setTX(MCP2515_MOSI);
+ SPI.setRX(MCP2515_MISO);
+ SPI.setCS(MCP2515_CS);
+ SPI.begin();
Serial.println("Configure ACAN2515");
ACAN2515Settings settings(QUARTZ_FREQUENCY, 100UL * 1000UL); // CAN bit rate 100 kb/s
settings.mRequestedMode = ACAN2515Settings::NormalMode ; // Select Normal mode
const uint16_t errorCode = can.begin(settings, onCanRecieve );
if (errorCode == 0) {
+ Serial.println("CAN Configured");
}
else{
Serial.print("Error: ");
- Serial.print(errorCode);
+ Serial.println(errorCode);
}
}
void onCanRecieve() {
@@ -186,13 +205,16 @@ void onCanRecieve() {
conbus_can.readCanMessage(frame.id, frame.data);
- switch (frame.id) {
+ printCanMsg(frame);
+
+ switch (frame.id) {
case 10:
- motorCommand = *(MotorCommand*)(frame.data); //Noah made me cry. I dont know what they did but I dont like it one bit - Jorge
+ motorCommand = *(MotorCommand*)(frame.data);
desired_forward_velocity = (float)motorCommand.setpoint_forward_velocity / SPEED_SCALE_FACTOR;
desired_angular_velocity = (float)motorCommand.setpoint_angular_velocity / SPEED_SCALE_FACTOR;
+
if (useObstacleAvoidance && isDetectingObstacle && desired_forward_velocity > 0) {
desired_forward_velocity = 0;
}
@@ -209,6 +231,7 @@ void onCanRecieve() {
drivetrain.setOutput(desired_forward_velocity, desired_angular_velocity);
break;
}
+
}
PIDSetpoints pid_setpoints;
@@ -251,12 +274,15 @@ void printCanMsg(CANMessage frame) {
}
Serial.println("");
}
+
void updateLeft(){
drivetrain.pulseLeftEncoder();
}
+
void updateRight(){
drivetrain.pulseRightEncoder();
}
+
void resetDelta(){
motorDistances.xn = 0;
motorDistances.yn = 0;
@@ -265,7 +291,7 @@ void resetDelta(){
delta_y = 0;
delta_theta = 0;
}
+
void updateTimers() {
motor_update_timer.update();
}
-
diff --git a/firmware/safety_lights/safety_lights.ino b/firmware/safety_lights/safety_lights.ino
index c37850eb..198004a5 100644
--- a/firmware/safety_lights/safety_lights.ino
+++ b/firmware/safety_lights/safety_lights.ino
@@ -2,29 +2,27 @@
#include
#include
#include "ArduinoJson.h"
-
-// Config
+#include
+#include
static const int BLINK_PERIOD_MS = 500;
static const int DEFAULT_BRIGHTNESS = 50;
-// Definitions
-
-static const int NUM_COLOR_LEDS = 61;
-static const int COLOR_PIN = 22;
-static const int E_STOP_PIN = 11;
-static const int WHITE_PIN = 18;
+static const int CLED_COUNT = 24;
+static const int CLED_PIN = 20;
+static const int ESTOP_PIN = 6;
+static const int W_PIN = 19;
-static const byte MCP2515_SCK = 2; // SCK input of MCP2515
-static const byte MCP2515_MOSI = 3; // SDI input of MCP2515
-static const byte MCP2515_MISO = 0; // SDO output of MCP2517
+static const byte MCP2515_SCK = 2;
+static const byte MCP2515_MOSI = 3;
+static const byte MCP2515_MISO = 0;
-static const byte MCP2515_CS = 1; // CS input of MCP2515
-static const byte MCP2515_INT = 4; // INT output of MCP2515
+static const byte MCP2515_CS = 1 ; // CS input of MCP2515 (adapt to your design)
+static const byte MCP2515_INT = 4 ; // INT output of MCP2515 (adapt to your design)
-Adafruit_NeoPixel strip(NUM_COLOR_LEDS, COLOR_PIN, NEO_GRB);
+Adafruit_NeoPixel strip(CLED_COUNT, CLED_PIN, NEO_GRB);
-// Start as mobility start and not autonomuos
+// Start as mobility start and not autonomous
bool is_estopped = false;
bool is_mobility_stopped = false;
bool is_autonomous = false;
@@ -35,63 +33,32 @@ int current_brightness = DEFAULT_BRIGHTNESS;
StaticJsonDocument<256> json_in;
// Default to fading purple as a "connecting" state
-int color_mode = 2;
+int color_mode = 0;
int current_color = strip.Color(0,255,255);
+// Setup CONBus variables
+CONBus::CONBus conbus;
+CONBus::CANBusDriver conbus_can(conbus, 13); // device id 13
+
+int current_blink_period = BLINK_PERIOD_MS; // how fast the light would blink per
+
typedef struct SafetyLightsMessage {
uint8_t autonomous: 1;
uint8_t eco: 1;
uint8_t mode: 6;
uint8_t brightness;
+
uint8_t red;
uint8_t green;
uint8_t blue;
+ uint8_t blink_period;
} SafetyLightsMessage;
SafetyLightsMessage last_CAN_message;
-ACAN2515 can(MCP2515_CS, SPI, MCP2515_INT);
-
-static const uint32_t QUARTZ_FREQUENCY = 8UL * 1000UL * 1000UL; // 8 MHZ
-
-uint32_t colorSelector(int num){ //0-15 to color
- switch(num){
- case 0: //black
- return strip.Color(0,0,0);
- case 1: //red
- return strip.Color(0,255,0);
- case 2: //orange
- return strip.Color(127,255,0);
- case 3: //yellow
- return strip.Color(255,255,0);
- case 4: //lime
- return strip.Color(255,127,0);
- case 5: //green
- return strip.Color(255,0,0);
- case 6: //cyan-green
- return strip.Color(255,0,127);
- case 7: //cyan
- return strip.Color(255,0,255);
- case 8: //light blue
- return strip.Color(127,0,255);
- case 9: //blue
- return strip.Color(0,0,255);
- case 10: //violet
- return strip.Color(0,127,255);
- case 11: //purple
- return strip.Color(0,255,255);
- case 12: //pink
- return strip.Color(0,255,127);
- case 13: //white green
- return strip.Color(255,127,127);
- case 14: //white red
- return strip.Color(127,255,127);
- case 15: //white
- return strip.Color(225,255,255);
- default:
- return strip.Color(0,0,0);
- }
-}
+ACAN2515 can (MCP2515_CS, SPI, MCP2515_INT) ;
+
+static const uint32_t QUARTZ_FREQUENCY = 8UL * 1000UL * 1000UL ; // 8 MHz
void modeSelector(){ // RGB mode selector
switch(color_mode){
@@ -104,6 +71,9 @@ void modeSelector(){ // RGB mode selector
case 2: //Fade
colorFade();
break;
+ // case 3: //party mode?
+ // partyTime();
+ // break;
default:
colorSolid();
break;
@@ -112,51 +82,67 @@ void modeSelector(){ // RGB mode selector
CANMessage frame;
-void onCanRecieve() {
+void onCanReceive() {
can.isr();
+ can.receive(frame);
+
+ conbus_can.readCanMessage(frame.id, frame.data);
}
-void setup() {
- Serial.begin (115200);
- delay(2000);
+void setup () {
+ pinMode(LED_BUILTIN, OUTPUT);
+ pinMode(W_PIN, OUTPUT);
+ digitalWrite(LED_BUILTIN, LOW);
+
+ Serial.begin();
SPI.setSCK(MCP2515_SCK);
SPI.setTX(MCP2515_MOSI);
SPI.setRX(MCP2515_MISO);
SPI.setCS(MCP2515_CS);
SPI.begin();
-
- pinMode(LED_BUILTIN, OUTPUT);
- pinMode(WHITE_PIN, OUTPUT);
- pinMode(E_STOP_PIN, INPUT);
- digitalWrite(LED_BUILTIN, HIGH);
-
+
strip.begin();
strip.show();
strip.setBrightness(current_brightness);
- analogWriteFreq(100);
-
- Serial.println("Configure ACAN2515");
- ACAN2515Settings settings(QUARTZ_FREQUENCY, 100UL * 1000UL); // CAN bit rate 100 kb/s
+ analogWriteFreq(100);
+ Serial.println ("Configure ACAN2515") ;
+ ACAN2515Settings settings (QUARTZ_FREQUENCY, 100UL * 1000UL) ; // CAN bit rate 100 kb/s
settings.mRequestedMode = ACAN2515Settings::NormalMode ; // Select Normal mode
- const uint16_t errorCode = can.begin(settings, onCanRecieve );
+ const uint16_t errorCode = can.begin (settings, [] { can.isr () ; }) ;
+
if (errorCode == 0) {
- Serial.println("ACAN Configured???");
+ Serial.println("L");
}
else{
- Serial.print("Error: ");
- Serial.print(errorCode);
+ Serial.print ("Configuration error 0x") ;
+ Serial.println (errorCode, HEX) ;
}
+ // Create CONBus registers
+ // The addresses can be anything from 0 to 255
+ // Both the blink period and the color of the leds
+ conbus.addRegister(7, ¤t_blink_period);
+ conbus.addRegister(99, ¤t_color);
+
}
void loop() {
-
// Serial.println("test");
+ // frame.id = 0x13;
+ // frame.len = 6;
+ // can.tryToSend (frame) ;
- is_estopped = !digitalRead(E_STOP_PIN);
+ is_estopped = !digitalRead(ESTOP_PIN);
+
+ if(is_estopped == false) {
+ digitalWrite(LED_BUILTIN, HIGH);
+ }
+ else {
+ digitalWrite(LED_BUILTIN, LOW);
+ }
if (Serial.available()) {
// Read the JSON document from the "link" serial port
@@ -172,6 +158,7 @@ void loop() {
is_eco = json_in["eco"].as();
color_mode = json_in["mode"].as();
current_brightness = json_in["brightness"].as();
+ current_blink_period = json_in["blink_period"].as();
current_color = strip.Color(json_green, json_red, json_blue);
}
else
@@ -184,56 +171,47 @@ void loop() {
if (can.available()) {
can.receive(frame);
- // Serial.print("Received CAN ");
- // Serial.println(frame.id);
+ Serial.print("Received CAN ");
+ Serial.println(frame.id);
switch (frame.id) {
case 1: // Mobility stop
is_mobility_stopped = true;
+ break;
case 9: // Mobility start
is_mobility_stopped = false;
+ break;
case 13: // Safety Lights
last_CAN_message = *(SafetyLightsMessage*)frame.data;
-
is_autonomous = last_CAN_message.autonomous;
is_eco = last_CAN_message.eco;
color_mode = last_CAN_message.mode;
current_brightness = last_CAN_message.brightness;
+ current_blink_period = 10*last_CAN_message.blink_period;
current_color = strip.Color(last_CAN_message.green, last_CAN_message.red, last_CAN_message.blue);
-
- // Serial.print(last_CAN_message.mode);
- // Serial.print(", ");
- // Serial.print(last_CAN_message.red);
- // Serial.print(", ");
- // Serial.print(last_CAN_message.green);
- // Serial.print(", ");
- // Serial.print(last_CAN_message.blue);
- // Serial.print(", ");
- // Serial.println(last_CAN_message.brightness);
+ break;
}
}
-
whiteFlash();
modeSelector();
-
- // delay(10); // Just so we aren't looping too fast
+ // delay(1000); // Just so we aren't looping too fast
}
void whiteFlash() {
if (!is_autonomous || is_estopped || is_mobility_stopped) {
if (!is_eco) {
- digitalWrite(WHITE_PIN, LOW);
+ digitalWrite(W_PIN, LOW);
} else {
- analogWrite(WHITE_PIN, 128);
+ analogWrite(W_PIN, 128);
}
return;
}
if (!is_eco) {
- digitalWrite(WHITE_PIN, (millis() / BLINK_PERIOD_MS) % 2);
+ digitalWrite(W_PIN, (millis() / current_blink_period) % 2);
} else {
- analogWrite(WHITE_PIN,((millis() / BLINK_PERIOD_MS) % 2) * 128);
+ analogWrite(W_PIN,(255-((millis() / current_blink_period) % 2) * 127));
}
}
@@ -245,7 +223,7 @@ void colorSolid() { // LED strip solid effect
void colorFlash(){ // LED strip flashing effect
strip.setBrightness(current_brightness);
- if ((millis() / BLINK_PERIOD_MS) % 2 == 0){
+ if ((millis() / current_blink_period) % 2 == 0){
strip.fill(current_color);
strip.show();
}
@@ -255,7 +233,7 @@ void colorFlash(){ // LED strip flashing effect
}
void colorFade(){ // LED strip fading effect
- strip.setBrightness(abs(sin(millis() / (float)BLINK_PERIOD_MS)) * current_brightness);
+ strip.setBrightness(abs(sin(millis() / (float)current_blink_period)) * current_brightness);
strip.fill(current_color);
strip.show();
}
@@ -264,3 +242,18 @@ void colorClear(){ // Clear LED strip
strip.fill(0);
strip.show();
}
+
+// void partyTime() // I love parties
+// {
+// strip.setBrightness(current_brightness);
+// if((millis() / current_blink_period / 2) % 2 == 0) {
+// for(int i=0;i
Camera / Filtered
+
+
diff --git a/display/scripts/globals.js b/display/scripts/globals.js
index d5742eba..dffd8daa 100644
--- a/display/scripts/globals.js
+++ b/display/scripts/globals.js
@@ -16,6 +16,8 @@ var deviceStates = {};
var logs = [];
var iterator = 0;
var iterators = [];
+var development_mode = false;
+var current_preset = "ERROR_NO_PRESET_AUTODETECTED";
var addressKeys = {
"autonav_serial_imu": {
@@ -23,25 +25,66 @@ var addressKeys = {
"imu_read_rate": "float"
},
- "autonav_serial_camera": {
- "internal_title": "[Serial] Camera",
+ "autonav_serial_camera_left": {
+ "internal_title": "[Serial] Left Camera",
"refresh_rate": "int",
"output_width": "int",
"output_height": "int",
- "camera_index": "int"
+ "scan_rate": "int",
+ "flip_horizontal": "bool",
+ "flip_vertical": "bool",
+ "rotate_clockwise": "bool"
+ },
+
+ "autonav_serial_camera_right": {
+ "internal_title": "[Serial] Right Camera",
+ "refresh_rate": "int",
+ "output_width": "int",
+ "output_height": "int",
+ "scan_rate": "int",
+ "flip_horizontal": "bool",
+ "flip_vertical": "bool",
+ "rotate_clockwise": "bool"
},
"autonav_vision_transformer": {
"internal_title": "[Vision] Transformer",
"lower_hue": "int",
- "lower_saturation": "int",
- "lower_value": "int",
+ "lower_sat": "int",
+ "lower_val": "int",
"upper_hue": "int",
- "upper_saturation": "int",
- "upper_value": "int",
- "blur": "int",
+ "upper_sat": "int",
+ "upper_val": "int",
+ "blur_weight": "int",
"blur_iterations": "int",
- "region_of_disinterest_offset": "int"
+ "map_res": "int",
+ "left_bottomleft": "point.int",
+ "left_bottomright": "point.int",
+ "left_topleft": "point.int",
+ "left_topright": "point.int",
+ "right_bottomleft": "point.int",
+ "right_bottomright": "point.int",
+ "right_topleft": "point.int",
+ "right_topright": "point.int",
+ "disable_blur": "bool",
+ "disable_hsv": "bool",
+ "disable_perspective_transform": "bool",
+ "disable_region_of_disinterest": "bool",
+ "parallelogram_left": "parallelogram.int",
+ "parallelogram_right": "parallelogram.int",
+ "top_width": "float",
+ "bottom_width": "float",
+ "offset": "float"
+ },
+
+ "autonav_vision_expandifier": {
+ "internal_title": "[Vision] Expandifier",
+ "horizontal_fov": "float",
+ "map_res": "int",
+ "vertical_fov": "float",
+ "max_range": "float",
+ "no_go_percent": "float",
+ "no_go_range": "float",
},
"autonav_filters": {
@@ -49,9 +92,7 @@ var addressKeys = {
"filter_type": {
0: "Deadreckoning",
1: "Particle Filter"
- },
- "degree_offset": "float",
- "seed_heading": "bool",
+ }
},
"autonav_manual_steamcontroller": {
@@ -61,13 +102,17 @@ var addressKeys = {
"throttle_deadzone": "float",
"turn_speed": "float",
"max_forward_speed": "float",
- "max_turn_speed": "float"
+ "max_turn_speed": "float",
+ "invert_steering": "bool",
+ "invert_throttle": "bool",
+ "throttle_rate": "float",
+ "steering_rate": "float"
},
"autonav_nav_astar": {
"internal_title": "[Navigation] A*",
- "pop_distance": "float",
- "direction": {
+ "waypointPopDistance": "float",
+ "waypointDirection": {
0: "North",
1: "South",
2: "Misc 1",
@@ -76,8 +121,13 @@ var addressKeys = {
5: "Misc 4",
6: "Misc 5",
},
- "use_only_waypoints": "bool",
- "waypoint_delay": "float",
+ "calculateWaypointDirection": "bool",
+ "useOnlyWaypoints": "bool",
+ "waypointDelay": "float",
+ "vertical_fov": "float",
+ "horizontal_fov": "float",
+ "waypointMaxWeight": "float",
+ "waypointWeight": "float",
},
"autonav_nav_resolver": {
@@ -91,18 +141,25 @@ var addressKeys = {
"max_angular_speed": "float"
},
- "autonav_playback": {
- "internal_title": "[Playback]",
- "record_imu": "bool",
- "record_gps": "bool",
- "record_position": "bool",
- "record_feedback": "bool",
- "record_objectdetection": "bool",
- "record_manual": "bool",
- "record_autonomous": "bool",
- "record_input": "bool",
- "record_debugfeedback": "bool",
- }
+ "autonav_image_combiner": {
+ "internal_title": "[Image Combiner]",
+ "map_res": "int"
+ },
+
+ "autonav_playback": {
+ "internal_title": "[Playback]",
+ "record_imu": "bool",
+ "record_gps": "bool",
+ "record_position": "bool",
+ "record_feedback": "bool",
+ "record_motor_debug": "bool",
+ "record_raw_cameras": "bool",
+ "record_filtered_cameras": "bool",
+ "record_astar": "bool",
+ "record_autonomous": "bool",
+ "record_manual": "bool",
+ "frame_rate": "int"
+ }
}
var conbusDevices = {
diff --git a/display/scripts/main.js b/display/scripts/main.js
index a0ed897d..036bcc58 100644
--- a/display/scripts/main.js
+++ b/display/scripts/main.js
@@ -24,6 +24,7 @@ $(document).ready(function () {
send({ op: "broadcast" });
send({ op: "get_nodes" });
+ send({ op: "get_presets" });
const waitInterval = setInterval(() => {
if (deviceStates["autonav_serial_can"] != 3) {
@@ -48,20 +49,49 @@ $(document).ready(function () {
$(".connecting").hide();
$("#main").show();
}, 1000);
+
+ setTimeout(() => {
+ if(websocket.readyState == 1)
+ {
+ send({ op: "get_presets" });
+ }
+ }, 3000);
};
websocket.onmessage = function (event) {
const messages = event.data.split("\n");
- for (const message of messages)
- {
+ for (const message of messages) {
const obj = JSON.parse(message);
const { op, topic } = obj;
-
+
if (op == "data") {
onTopicData(topic, obj);
}
-
+
+ if (op == "get_presets_callback") {
+ const presets = obj.presets;
+ const presetElement = $("#dropdown_elements");
+ presetElement.empty();
+ for (const preset of presets) {
+ const dropdownItem = $(`
+
- + Active Preset: None +
+
+
+
+
+
+
+
+
+
+
+
+
- ![]()
- ![]()
- ![]()
+
-
+
-
+
@@ -210,9 +247,21 @@
+ System
+
+
+
+
-
@@ -230,7 +279,7 @@ System
Loading...
- Waiting for the Weeb Wagon
+Waiting for the Danger Zone
${id}: ${deviceStateToName(state)}
`); + } + } } + + for (const key in obj.configs) { + config[key] = obj.configs[key]; + } + regenerateConfig(); + + // Update system state + let system = obj["system"]; + $("#var_system_state").text(system["state"] == 0 ? "Diabled" : system["state"] == 1 ? "Autonomous" : system["state"] == 2 ? "Manual" : "Shutdown"); + $("#var_system_mode").text(system["mode"] == 0 ? "Competition" : system["mode"] == 1 ? "Simulation" : "Practice"); + $("#var_system_mobility").text(system["mobility"] ? "Enabled" : "Disabled"); + + // Update some buttons + $("#checkbox_system_mobility").prop("checked", system["mobility"]); + $("#input_system_state").val(system["state"]); } } }; websocket.onclose = function (event) { - $("#connecting-state").text("Waiting for the Weeb Wagon"); + $("#connecting-state").text("Waiting for the Danger Zone"); $(".connecting").show(); $(".connecting-input").show(); $("#main").hide(); @@ -93,165 +153,26 @@ $(document).ready(function () { }; } - createWebsocket(); + if (!development_mode) { + createWebsocket(); + } else { + $("#connecting-state").text("Updating Data"); + $(".connecting-input").hide(); + $(".connecting").hide(); + $("#main").show(); + } var sendQueue = []; - function send(obj) { - sendQueue.push(obj); - } - function setSystemState() { send({ op: "set_system_state", state: systemState.state, - estop: systemState.estop, + mode: systemState.mode, mobility: systemState.mobility, }); } - function generateElementForConfiguration(data, type, device, text) { - if (type == "bool") { - const checked = fromBytesToBool(data); - - // Create a dropdown - const div = document.createElement("div"); - div.classList.add("input-group"); - div.classList.add("mb-3"); - - const select = document.createElement("select"); - select.classList.add("form-select"); - select.onchange = function () { - send({ - op: "configuration", - opcode: 1, - device: device, - address: text, - data: Array.from(fromBoolToBytes(select.value == 1)), - iterator: iterate() - }); - } - - const optionTrue = document.createElement("option"); - optionTrue.value = 1; - optionTrue.innerText = "True"; - optionTrue.selected = checked; - - const optionFalse = document.createElement("option"); - optionFalse.value = 0; - optionFalse.innerText = "False"; - optionFalse.selected = !checked; - - select.appendChild(optionTrue); - select.appendChild(optionFalse); - - const span = document.createElement("span"); - span.classList.add("input-group-text"); - span.innerText = text; - - div.appendChild(span); - div.appendChild(select); - return div; - } - else if (type == "float") { - const div = document.createElement("div"); - div.classList.add("input-group"); - div.classList.add("mb-3"); - - const input = document.createElement("input"); - input.type = "number"; - input.classList.add("form-control"); - input.value = fromBytesToFloat(data).toFixed(6); - input.onchange = function () { - send({ - op: "configuration", - opcode: 1, - device: device, - address: text, - data: Array.from(fromFloatToBytes(input.value)), - iterator: iterate() - }); - } - - const span = document.createElement("span"); - span.classList.add("input-group-text"); - span.innerText = text; - - div.appendChild(span); - div.appendChild(input); - return div; - } - else if (type == "int") { - const div = document.createElement("div"); - div.classList.add("input-group"); - div.classList.add("mb-3"); - - const input = document.createElement("input"); - input.type = "number"; - input.classList.add("form-control"); - input.value = fromBytesToInt(data); - input.onchange = function () { - send({ - op: "configuration", - opcode: 1, - device: device, - address: text, - data: Array.from(fromIntToBytes(input.value)), - iterator: iterate() - }); - } - - const span = document.createElement("span"); - span.classList.add("input-group-text"); - span.innerText = text; - - div.appendChild(span); - div.appendChild(input); - return div; - } - else { - const options = addressKeys[device][text]; - - if (typeof options == "object") { - const index = fromBytesToInt(data); - - // Create a dropdown - const div = document.createElement("div"); - div.classList.add("input-group"); - div.classList.add("mb-3"); - - const select = document.createElement("select"); - select.classList.add("form-select"); - select.onchange = function () { - send({ - op: "configuration", - opcode: 1, - device: device, - address: text, - data: Array.from(fromIntToBytes(select.value)), - iterator: iterate() - }); - } - - for (let i = 0; i < Object.keys(options).length; i++) { - const option = document.createElement("option"); - option.value = i; - option.selected = i == index; - option.innerText = options[i]; - select.appendChild(option); - } - - const span = document.createElement("span"); - span.classList.add("input-group-text"); - span.innerText = text; - - div.appendChild(span); - div.appendChild(select); - return div; - } - } - } - function generateElementForConbus(data, type, text, deviceId, address, readonly = false) { if (type == "bool") { const checked = fromBytesToBool(data); @@ -408,22 +329,19 @@ $(document).ready(function () { } if (topic == "/scr/state/system") { - const { state, mode, mobility, estop } = msg; + const { state, mode, mobility } = msg; $("#var_system_state").text(state == 0 ? "Diabled" : state == 1 ? "Autonomous" : state == 2 ? "Manual" : "Shutdown"); $("#var_system_mode").text(mode == 0 ? "Competition" : mode == 1 ? "Simulation" : "Practice"); $("#var_system_mobility").text(mobility ? "Enabled" : "Disabled"); - $("#var_system_estop").text(estop ? "Yes" : "No"); systemState.state = state; systemState.mode = mode; systemState.mobility = mobility; - systemState.estop = estop; $("#input_system_state").val(state); $("#input_system_mode").val(mode); $("#input_system_mobility").prop("checked", mobility); - $("#input_system_estop").prop("checked", estop); return; } @@ -441,54 +359,9 @@ $(document).ready(function () { } if (topic == "/scr/configuration") { - const { device, opcode, data, address } = msg; - if (opcode == 4) { - return; - } - - if (opcode == 2 || opcode == 3) { - if (!(device in config)) { - config[device] = {}; - } - - config[device][address] = data; - - const configElement = $("#configuration"); - configElement.empty(); - - for (const deviceId in config) { - const deviceConfig = config[deviceId]; - const title = addressKeys[deviceId]["internal_title"]; - const deviceElement = $(``); - deviceElement.append(`${title}
Unknown Address: ${address}
`);
- deviceBody.append(alert);
- continue;
- }
-
- const inputElement = generateElementForConfiguration(data, type, deviceId, address);
- deviceBody.append(inputElement);
- }
-
- for (const address in addressKeys[deviceId]) {
- if (address in deviceConfig || address == "internal_title") {
- continue;
- }
-
- const alert = $(`Unknown Address: ${address}
`);
- deviceBody.append(alert);
- }
-
- configElement.append(deviceElement);
- }
- }
+ const { device, json } = msg;
+ config[device] = JSON.parse(json);
+ regenerateConfig();
return;
}
@@ -560,22 +433,33 @@ $(document).ready(function () {
return;
}
- if (topic == "/autonav/camera/compressed") {
- // Set to
- const imgElement = document.getElementById("target_raw_camera");
- imgElement.src = `data:image/jpeg;base64,${msg.data}`;
+ if (topic == "/autonav/camera/compressed/left") {
+ transferImageToElement("target_raw_camera_left", msg.data);
+ return;
+ }
+
+ if (topic == "/autonav/camera/compressed/right") {
+ transferImageToElement("target_raw_camera_right", msg.data);
+ return;
+ }
+
+ if (topic == "/autonav/cfg_space/raw/image/left_small") {
+ transferImageToElement("target_filtered_left", msg.data);
+ return;
+ }
+
+ if (topic == "/autonav/cfg_space/raw/image/right_small") {
+ transferImageToElement("target_filtered_right", msg.data);
return;
}
- if (topic == "/autonav/cfg_space/raw/image") {
- const imgElement = document.getElementById("target_filtered_camera");
- imgElement.src = `data:image/jpeg;base64,${msg.data}`;
+ if (topic == "/autonav/cfg_space/combined/image") {
+ transferImageToElement("target_combined", msg.data);
return;
}
if (topic == "/autonav/debug/astar/image") {
- const imgElement = document.getElementById("target_astar_path");
- imgElement.src = `data:image/jpeg;base64,${msg.data}`;
+ transferImageToElement("target_astar", msg.data);
return;
}
@@ -651,6 +535,7 @@ $(document).ready(function () {
$(".dropdown-menu a").on("click", function () {
const parentDataTarget = $(this).parents(".dropdown").attr("data-target");
+ console.log(parentDataTarget);
if (parentDataTarget == "system_state") {
const id = $(this).attr("data-value");
systemState.state = parseInt(id);
@@ -670,9 +555,29 @@ $(document).ready(function () {
}
});
- $("#checkbox_system_estop").on("change", function () {
- systemState.estop = $(this).is(":checked");
- setSystemState();
+ $("#save_preset_mode").on("click", function () {
+ send({
+ op: "save_preset_mode"
+ });
+ send({ op: "get_presets" });
+ });
+
+ $("#save_preset_as").on("click", function () {
+ const preset_name = $("#preset_save_name").val();
+ send({
+ op: "save_preset_as",
+ preset: preset_name
+ });
+ send({ op: "get_presets" });
+ $("#preset_save_name").val("");
+ });
+
+ $("#delete_preset").on("click", function () {
+ send({
+ op: "delete_preset",
+ preset: current_preset
+ });
+ send({ op: "get_presets" });
});
$("#checkbox_system_mobility").on("change", function () {
@@ -701,4 +606,363 @@ $(document).ready(function () {
logs = [];
$("#log_body").empty();
});
+
+ function generateElementForConfiguration(data, type, device, text) {
+ if (type == "bool") {
+ const checked = data == 1;
+
+ // Create a dropdown
+ const div = document.createElement("div");
+ div.classList.add("input-group");
+ div.classList.add("mb-3");
+
+ const select = document.createElement("select");
+ select.classList.add("form-select");
+ select.onchange = function () {
+ config[device][text] = select.value == 1 ? true : false;
+ send({
+ op: "configuration",
+ device: device,
+ json: config[device],
+ });
+ }
+
+ const optionTrue = document.createElement("option");
+ optionTrue.value = 1;
+ optionTrue.innerText = "True";
+ optionTrue.selected = checked;
+
+ const optionFalse = document.createElement("option");
+ optionFalse.value = 0;
+ optionFalse.innerText = "False";
+ optionFalse.selected = !checked;
+
+ select.appendChild(optionTrue);
+ select.appendChild(optionFalse);
+
+ const span = document.createElement("span");
+ span.classList.add("input-group-text");
+ span.innerText = text;
+
+ div.appendChild(span);
+ div.appendChild(select);
+ return div;
+ }
+ else if (type == "float") {
+ const div = document.createElement("div");
+ div.classList.add("input-group");
+ div.classList.add("mb-3");
+
+ const input = document.createElement("input");
+ input.type = "number";
+ input.classList.add("form-control");
+ input.value = data;
+ input.onchange = function () {
+ config[device][text] = parseFloat(input.value);
+ send({
+ op: "configuration",
+ device: device,
+ json: config[device],
+ });
+ }
+
+ const span = document.createElement("span");
+ span.classList.add("input-group-text");
+ span.innerText = text;
+
+ div.appendChild(span);
+ div.appendChild(input);
+ return div;
+ }
+ else if (type == "int") {
+ const div = document.createElement("div");
+ div.classList.add("input-group");
+ div.classList.add("mb-3");
+
+ const input = document.createElement("input");
+ input.type = "number";
+ input.classList.add("form-control");
+ input.value = data;
+ input.onchange = function () {
+ config[device][text] = parseInt(input.value);
+ send({
+ op: "configuration",
+ device: device,
+ json: config[device],
+ });
+ }
+
+ const span = document.createElement("span");
+ span.classList.add("input-group-text");
+ span.innerText = text;
+
+ div.appendChild(span);
+ div.appendChild(input);
+ return div;
+ }
+ else if (type == "point.int") {
+ // x, y point for two integers
+ const div = document.createElement("div");
+ div.classList.add("input-group");
+ div.classList.add("mb-3");
+
+ const inputX = document.createElement("input");
+ inputX.type = "number";
+ inputX.classList.add("form-control");
+ inputX.value = data[0];
+ inputX.onchange = function () {
+ config[device][text][0] = parseInt(inputX.value);
+ send({
+ op: "configuration",
+ device: device,
+ json: config[device],
+ });
+ }
+
+ const inputY = document.createElement("input");
+ inputY.type = "number";
+ inputY.classList.add("form-control");
+ inputY.value = data[1];
+ inputY.onchange = function () {
+ config[device][text][1] = parseInt(inputY.value);
+ send({
+ op: "configuration",
+ device: device,
+ json: config[device],
+ });
+ }
+
+ const span = document.createElement("span");
+ span.classList.add("input-group-text");
+ span.innerText = text;
+
+ div.appendChild(span);
+ div.appendChild(inputX);
+ div.appendChild(inputY);
+ return div;
+ }
+ else if (type == "parallelogram.int") {
+ const div = document.createElement("div");
+ div.classList.add("input-group", "mb-3");
+
+ function createCoordinateInput(value, onChangeHandler) {
+ const input = document.createElement("input");
+ input.type = "number";
+ input.classList.add("form-control", "coordinate-input");
+ input.value = value;
+ input.onchange = onChangeHandler;
+ return input;
+ }
+
+ const inputX1 = createCoordinateInput(data[0][0], function () {
+ config[device][text][0][0] = parseInt(inputX1.value);
+ send({
+ op: "configuration",
+ device: device,
+ json: config[device],
+ });
+ });
+
+ const inputY1 = createCoordinateInput(data[0][1], function () {
+ config[device][text][0][1] = parseInt(inputY1.value);
+ send({
+ op: "configuration",
+ device: device,
+ json: config[device],
+ });
+ });
+
+ const inputX2 = createCoordinateInput(data[1][0], function () {
+ config[device][text][1][0] = parseInt(inputX2.value);
+ send({
+ op: "configuration",
+ device: device,
+ json: config[device],
+ });
+ });
+
+ const inputY2 = createCoordinateInput(data[1][1], function () {
+ config[device][text][1][1] = parseInt(inputY2.value);
+ send({
+ op: "configuration",
+ device: device,
+ json: config[device],
+ });
+ });
+
+ const inputX3 = createCoordinateInput(data[2][0], function () {
+ config[device][text][2][0] = parseInt(inputX3.value);
+ send({
+ op: "configuration",
+ device: device,
+ json: config[device],
+ });
+ });
+
+ const inputY3 = createCoordinateInput(data[2][1], function () {
+ config[device][text][2][1] = parseInt(inputY3.value);
+ send({
+ op: "configuration",
+ device: device,
+ json: config[device],
+ });
+ });
+
+ const inputX4 = createCoordinateInput(data[3][0], function () {
+ config[device][text][3][0] = parseInt(inputX4.value);
+ send({
+ op: "configuration",
+ device: device,
+ json: config[device],
+ });
+ });
+
+ const inputY4 = createCoordinateInput(data[3][1], function () {
+ config[device][text][3][1] = parseInt(inputY4.value);
+ send({
+ op: "configuration",
+ device: device,
+ json: config[device],
+ });
+ });
+
+ const span = document.createElement("span");
+ span.classList.add("input-group-text");
+ span.innerText = text;
+
+ div.appendChild(span);
+ div.appendChild(inputX1);
+ div.appendChild(inputY1);
+ div.appendChild(inputX2);
+ div.appendChild(inputY2);
+ div.appendChild(inputX3);
+ div.appendChild(inputY3);
+ div.appendChild(inputX4);
+ div.appendChild(inputY4);
+ return div;
+ }
+ else {
+ const options = addressKeys[device][text];
+
+ if (typeof options == "object") {
+ const index = data;
+
+ // Create a dropdown
+ const div = document.createElement("div");
+ div.classList.add("input-group");
+ div.classList.add("mb-3");
+
+ const select = document.createElement("select");
+ select.classList.add("form-select");
+ select.onchange = function () {
+ config[device][text] = parseInt(select.value);
+ send({
+ op: "configuration",
+ device: device,
+ json: config[device],
+ });
+ }
+
+ for (let i = 0; i < Object.keys(options).length; i++) {
+ const option = document.createElement("option");
+ option.value = i;
+ option.selected = i == index;
+ option.innerText = options[i];
+ select.appendChild(option);
+ }
+
+ const span = document.createElement("span");
+ span.classList.add("input-group-text");
+ span.innerText = text;
+
+ div.appendChild(span);
+ div.appendChild(select);
+ return div;
+ }
+ }
+ }
+
+ const regenerateConfig = () => {
+ const configElement = $("#options");
+ configElement.empty();
+
+ // Sort the keys in each config by their addressKeys
+ for(const deviceId in addressKeys)
+ {
+ if (!(deviceId in config)) {
+ continue;
+ }
+
+ const title = addressKeys[deviceId]["internal_title"];
+ const deviceElement = $(``);
+ deviceElement.append(`${title}
Key not found: ${address}
`);
+ deviceBody.append(alert);
+ continue;
+ }
+
+ const data = deviceConfig[address];
+ const type = addressKeys[deviceId][address];
+ const inputElement = generateElementForConfiguration(data, type, deviceId, address);
+ deviceBody.append(inputElement);
+ }
+
+ configElement.append(deviceElement);
+ }
+
+ // config = outputConfig;
+ // for (const deviceId in config) {
+ // const deviceConfig = config[deviceId];
+ // if (addressKeys[deviceId] == undefined) {
+ // console.log(`Unknown Device Config: ${deviceId}`);
+ // // const alert = $(`Unknown Device Config: ${deviceId}
`);
+ // // configElement.append(alert);
+ // continue;
+ // }
+
+ // const title = addressKeys[deviceId]["internal_title"];
+ // const deviceElement = $(``);
+ // deviceElement.append(`${title}
Unknown Type: ${address}
`);
+ // deviceBody.append(alert);
+ // continue;
+ // }
+
+ // const inputElement = generateElementForConfiguration(data, type, deviceId, address);
+ // deviceBody.append(inputElement);
+ // }
+
+ // for (const address in addressKeys[deviceId]) {
+ // if (address in deviceConfig || address == "internal_title") {
+ // continue;
+ // }
+
+ // const alert = $(`Unknown Configuration Entry: ${address}
`);
+ // deviceBody.append(alert);
+ // }
+
+ // configElement.append(deviceElement);
+ // }
+ }
+
+ function send(obj) {
+ sendQueue.push(obj);
+ }
})
\ No newline at end of file
diff --git a/display/scripts/utilities.js b/display/scripts/utilities.js
index 0bc62169..0108877c 100644
--- a/display/scripts/utilities.js
+++ b/display/scripts/utilities.js
@@ -23,11 +23,29 @@ const fromBytesToBool = (bytes) => {
return getBytesView(bytes).getUint8(0) == 1;
}
-const transferImageToElement = (id, data) => {
+const transferImageToElementOld = (id, data) => {
const img = document.getElementById(id);
img.src = "data:image/jpeg;base64," + data;
}
+const transferImageToElement = (id, data) => {
+ const img = document.getElementById(id);
+ const uint8Array = new Uint8Array(data);
+ const blob = new Blob([uint8Array], { type: "image/jpeg" });
+ const urlCreator = window.URL || window.webkitURL;
+ const imageUrl = urlCreator.createObjectURL(blob);
+ img.src = imageUrl;
+}
+
+const trasferImageBytesToElement = (id, data) => {
+ const img = document.getElementById(id);
+ const msgarray = new Uint8Array(data);
+ const blob = new Blob([msgarray], { type: "image/jpeg" });
+ const urlCreator = window.URL || window.webkitURL;
+ const imageUrl = urlCreator.createObjectURL(blob);
+ img.src = imageUrl;
+}
+
const fromFloatToBytes = (value) => {
var buffer = new ArrayBuffer(4);
var view = new DataView(buffer);
@@ -121,7 +139,7 @@ const radiansToDegrees = (radians) => {
}
const deviceStateToName = (state) => {
- return state == 0 ? "Off" : state == 1 ? "Standby" : state == 2 ? "Ready" : "Operating";
+ return ["Off", "Booting", "Standby", "Ready", "Operating", "Errored"][state];
}
const clearGlobals = () => {
diff --git a/display/styles/index.css b/display/styles/index.css
index 0b6edefb..db90d939 100644
--- a/display/styles/index.css
+++ b/display/styles/index.css
@@ -76,20 +76,34 @@ h5 > span {
flex-direction: column;
}
+/* Off */
span[data-state="0"] {
color: --var(-text-color);
}
+/* Booting */
span[data-state="1"] {
- color: #C21858;
+ color: #FF9800;
}
+/* Standby */
span[data-state="2"] {
- color: #57ACDC;
+ color: #FFC107;
}
+/* Ready */
span[data-state="3"] {
- color: #60C689;
+ color: #4CAF50;
+}
+
+/* Operating */
+span[data-state="4"] {
+ color: #57ACDC;
+}
+
+/* Errored */
+span[data-state="5"] {
+ color: #C21858;
}
#element_device_states > h5 > span {
@@ -127,6 +141,30 @@ span[data-state="3"] {
margin: 1rem 0;
}
+.controls-row {
+ display: flex;
+ flex-direction: row;
+ gap: 1rem;
+}
+
+.controls-row > button {
+ text-wrap: nowrap;
+}
+
+#controls {
+ display: flex;
+ flex-direction: column;
+ gap: 1rem;
+ width: fit-content;
+}
+
+#options {
+ display: flex;
+ flex-direction: column;
+ gap: 1rem;
+ margin: 1rem 0;
+}
+
#preferences {
display: flex;
flex-direction: column;
@@ -139,21 +177,55 @@ span[data-state="3"] {
#images {
display: flex;
- flex-direction: row;
+ flex-direction: column;
gap: 1rem;
- flex-wrap: wrap;
}
-#images > canvas[data-type="regular"] {
- width: 640px !important;
- height: 480px !important;
+img[data-type="regular"] {
+ width: 480px !important;
+ height: 640px !important;
}
-#images > canas[data-type="bigboy"] {
+img[data-type="bigboy"] {
width: 800px !important;
height: 800px !important;
}
+img[data-type="small"] {
+ width: 320px !important;
+ height: 320px !important;
+}
+
#logging {
margin: 1rem;
+}
+
+.coordinate-input {
+ width: 60px;
+ margin-right: 5px;
+ text-align: center;
+ border-radius: 5px;
+ border: 1px solid #ced4da;
+}
+
+.roww {
+ display: flex;
+ flex-direction: row;
+ flex-wrap: wrap;
+ width: fit-content;
+ height: min-content;
+}
+
+#dashboard {
+ display: flex;
+ flex-wrap: wrap;
+ gap: 1rem;
+}
+
+#vtest {
+ min-width: 1000px;
+}
+
+#active_preset_value {
+ color: #ff7bff;
}
\ No newline at end of file
diff --git a/firmware/motor_control/CANBusDriver.h b/firmware/motor_control/CANBusDriver.h
new file mode 100644
index 00000000..35789d87
--- /dev/null
+++ b/firmware/motor_control/CANBusDriver.h
@@ -0,0 +1,146 @@
+#ifndef CANBUS_DRIVER_H
+#define CANBUS_DRIVER_H
+
+#include