first commit

src/B_G431B_ESC1_motor_controller/B_G431B_ESC1_motor_controller.ino

@@ -0,0 +1,145 @@
+/** 
+ * B-G431B-ESC1 position motion control example with encoder
+ *
+ */
+#include <SimpleFOC.h>
+#include <SimpleFOCDrivers.h>
+#include <encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h>
+
+// Motor instance
+//BLDCMotor motor = BLDCMotor(7, 0.14, 2450, 0.00003);
+//BLDCMotor motor = BLDCMotor(7, 0.15, 940, 0.000125);
+BLDCMotor motor = BLDCMotor(7, 0.14, 850, 0.0000425);
+BLDCDriver6PWM driver = BLDCDriver6PWM(A_PHASE_UH, A_PHASE_UL, A_PHASE_VH, A_PHASE_VL, A_PHASE_WH, A_PHASE_WL);
+// Gain calculation shown at https://community.simplefoc.com/t/b-g431b-esc1-current-control/521/21
+LowsideCurrentSense currentSense = LowsideCurrentSense(0.003f, -64.0f/7.0f, A_OP1_OUT, A_OP2_OUT, A_OP3_OUT);
+
+// encoder instance
+MXLEMMINGObserverSensor sensor = MXLEMMINGObserverSensor(motor);
+
+HardwareSerial Serial1(PB6, PB7);
+
+// velocity set point variable
+float target = 0;
+float targetP = 0;
+float targetI = 0;
+float targetV = 0;
+// instantiate the commander
+Commander commandComp = Commander(Serial);
+Commander commandESP = Commander(Serial1);
+void doTargetP(char* cmd) { commandComp.scalar(&targetP, cmd); }
+void doTargetI(char* cmd) { commandComp.scalar(&targetI, cmd); }
+void doTargetV(char* cmd) { commandESP.scalar(&targetV, cmd); }
+
+void setup() {
+
+  //pinMode(LED_RED, OUTPUT); // Set board LED for debugging
+  //digitalWrite(LED_RED, HIGH);
+  
+  // use monitoring with serial 
+  Serial.begin(115200);
+  Serial1.begin(115200);
+
+  // enable more verbose output for debugging
+  // comment out if not needed
+  SimpleFOCDebug::enable(&Serial);
+
+  // initialize encoder sensor hardware
+  sensor.init();
+
+  // link the motor to the sensor
+  motor.linkSensor(&sensor);
+  
+  // driver config
+  // power supply voltage [V]
+  driver.voltage_power_supply = 12;
+  driver.init();
+  // link the motor and the driver
+  motor.linkDriver(&driver);
+  // link current sense and the driver
+  currentSense.linkDriver(&driver);
+
+  // initialise the current sensing
+  if(!currentSense.init()){
+    Serial.println("Current sense init failed.");
+    return;
+  }
+  // no need for aligning
+  currentSense.skip_align = true;
+  motor.linkCurrentSense(&currentSense);
+
+  motor.sensor_direction= Direction::CW;
+  motor.zero_electric_angle = 0;
+
+  // set motion control loop to be used
+  motor.controller = MotionControlType::velocity;
+  motor.torque_controller = TorqueControlType::foc_current;
+  //motor.torque_controller = TorqueControlType::dc_current;
+  //motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
+
+  motor.current_sp = motor.current_limit;
+
+  // velocity PI controller parameters
+  motor.PID_velocity.P = 0.01;
+  motor.PID_velocity.I = 0.004;
+
+  driver.voltage_limit = 8;
+  motor.voltage_limit = 4;
+  //motor.current_limit = 2;
+
+  motor.PID_velocity.limit = motor.voltage_limit;
+
+  // jerk control using voltage voltage ramp
+  // default value is 300 volts per sec  ~ 0.3V per millisecond
+  motor.PID_velocity.output_ramp = 10;
+ 
+  // velocity low pass filtering time constant
+  //motor.LPF_velocity.Tf = 0.1;
+
+  // comment out if not needed
+  motor.useMonitoring(Serial);
+
+  motor.monitor_downsample = 10000;
+  //motor.motion_downsample = 4;
+  
+  // initialize motor
+  motor.init();
+  // align encoder and start FOC
+  motor.initFOC();
+
+  // add target command T
+  commandComp.add('P', doTargetP, "target P");
+  commandComp.add('I', doTargetI, "target I");
+  commandESP.add('V', doTargetV, "target V");
+
+  Serial.println(F("Motor ready."));
+  Serial.println(F("Set the target velocity using serial terminal:"));
+  _delay(1000);
+}
+
+void loop() {
+  // main FOC algorithm function
+  motor.loopFOC();
+
+  //motor.PID_velocity.P = targetP;
+  //motor.PID_velocity.I = targetI;
+
+  //Serial.println(target);
+
+  // Motion control function
+  motor.move(targetV);
+
+  if (abs(motor.target) < 20.0f && motor.enabled==1)
+    motor.disable();
+  if (abs(motor.target) >= 20.0f && motor.enabled==0)
+    motor.enable();
+
+  // function intended to be used with serial plotter to monitor motor variables
+  // significantly slowing the execution down!!!!
+  motor.monitor();
+  
+  // user communication
+  commandESP.run();
+  commandComp.run();
+
+}

src/B_G431B_ESC1_motor_controller/build_opt.h

@@ -0,0 +1,2 @@
+-DHAL_OPAMP_MODULE_ENABLED
+-DHAL_UART_MODULE_ENABLED

src/siren_controller_esp32_simple_station/siren_controller_esp32_simple_station.ino

@@ -0,0 +1,222 @@
+#include "WiFi.h"
+
+#include <Wire.h>
+
+// Display Libraries
+#include <Wire.h>
+#include <SSD1306Ascii.h>
+#include <SSD1306AsciiWire.h>
+
+// Other Libraries
+#include <arduino-timer.h>
+#include <AiEsp32RotaryEncoder.h>
+#include <ArduinoOSCWiFi.h>
+
+// WiFi stuff
+const char* ssid = "sirening";
+const char* pwd = "alarm_11735";
+const IPAddress ip(192, 168, 4, 201);
+const IPAddress gateway(192, 168, 4, 1);
+const IPAddress subnet(255, 255, 0, 0);
+
+// for ArduinoOSC
+const char* host = "192.168.4.200";
+const int settings_port = 54000;
+const int state_port = 54001;
+
+// Button Defs
+bool freqButtonState = false;
+AiEsp32RotaryEncoder freqEncoderButton = AiEsp32RotaryEncoder(34, 35, 32, -1, 4);
+bool ampButtonState = false;
+AiEsp32RotaryEncoder ampEncoderButton = AiEsp32RotaryEncoder(33, 25, 26, -1, 4);
+
+float frequency = 0;
+float amplitude = 0;
+const int ports = 6;
+const int motorIndex = 0;
+
+// Display Defs
+#define I2C_ADDRESS 0x3C
+SSD1306AsciiWire oled;
+uint8_t fieldWidth;
+uint8_t rowsPerLine; // Rows per line.
+
+HardwareSerial freqSerial(1);
+HardwareSerial ampSerial(2);
+
+void killAll() {
+  updateFreq(0.0);
+  updateAmp(0.0);
+}
+
+float hzToRads(int freq){
+  return freq * TWO_PI / ports;
+}
+
+float rpmToRads(int rpm){
+  return (rpm / 60) * TWO_PI;
+}
+
+void updateFreq(float val){
+  frequency = val;
+  Serial.println("new freq is: " + String(frequency));
+  freqEncoderButton.setEncoderValue(frequency * 10.0);
+  freqSerial.print("\nV" + String(hzToRads(frequency)) + "\n");
+  updateDisplay();
+}
+
+void updateAmp(float val){
+  amplitude = val;
+  Serial.println("new amp is: " + String(amplitude));
+  ampEncoderButton.setEncoderValue(amplitude * 10.0);
+  ampSerial.print("\nV" + String(hzToRads(amplitude)) + "\n");
+  updateDisplay();
+}
+
+void setupOSC(){
+  OscWiFi.publish(host, state_port, "/state", motorIndex, frequency, amplitude)->setFrameRate(10.f);
+
+  OscWiFi.subscribe(settings_port, "/kill", [](const OscMessage& m) {
+    killAll();
+  });
+
+  OscWiFi.subscribe(settings_port, "/freq", [](const OscMessage& m) {
+    updateFreq(m.arg<float>(0));
+  });
+
+  OscWiFi.subscribe(settings_port, "/amp", [](const OscMessage& m) {
+    updateAmp(m.arg<float>(0));
+  });
+}
+
+void IRAM_ATTR readFreqEncoderISR() {
+  freqEncoderButton.readEncoder_ISR();
+}
+
+void IRAM_ATTR readAmpEncoderISR() {
+  ampEncoderButton.readEncoder_ISR();
+}
+
+void setupEncoderButton(AiEsp32RotaryEncoder& eb, char* val){
+  eb.begin();
+  if(val == "freq") {
+    eb.setup(readFreqEncoderISR);
+  } else {
+    eb.setup(readAmpEncoderISR);
+  }
+  eb.setBoundaries(0, 4000, false);
+  eb.setAcceleration(1000);
+}
+
+void initWiFi() {
+  WiFi.mode(WIFI_STA);
+  WiFi.begin(ssid, pwd);
+  WiFi.config(ip, gateway, subnet);
+  Serial.println();
+  Serial.print("Connecting to WiFi ..");
+  while (WiFi.status() != WL_CONNECTED) {
+    Serial.print('.');
+    delay(1000);
+  }
+  Serial.println(WiFi.localIP());
+}
+
+void setup() {
+
+  Serial.begin(115200);
+  freqSerial.begin(115200, SERIAL_8N1, 16, 17);
+  ampSerial.begin(115200, SERIAL_8N1, 18, 19);
+  delay(2000);
+
+  initWiFi();
+
+  setupOSC();
+
+  setupEncoderButton(freqEncoderButton, "freq");
+  setupEncoderButton(ampEncoderButton, "amp");
+
+  Wire.begin();
+  oled.begin(&Adafruit128x64, I2C_ADDRESS);
+  //oled.setI2cClock(100000L);
+  oled.setFont(System5x7);
+  oled.clear();
+  oled.println();
+  oled.println();
+  oled.println("  frequency: 0");
+  oled.print("  amplitude: 0");
+  fieldWidth = oled.fieldWidth(strlen("  amplitude: "));
+  rowsPerLine = oled.fontRows();
+}
+
+void encoderButtonUpdate(char* val, float& setting, AiEsp32RotaryEncoder& eb, bool& buttonState, HardwareSerial& ser) {
+  if (eb.encoderChanged()) {
+    setting = eb.readEncoder() / 10.0;
+    ser.print("\nV" + String(hzToRads(setting)) + "\n");
+    Serial.println("new " + String(val) + " is: " + String(setting));
+    updateDisplay();
+  }
+
+  if (eb.isEncoderButtonDown() && !buttonState) {
+    buttonState = true;
+  }
+
+  //button is up
+  if (!eb.isEncoderButtonDown() && buttonState) {
+    killAll();
+    buttonState = false;
+  }
+}
+
+void loop()
+{
+  encoderButtonUpdate("freq", frequency, freqEncoderButton, freqButtonState, freqSerial);
+  encoderButtonUpdate("amp", amplitude, ampEncoderButton, ampButtonState, ampSerial);
+
+  OscWiFi.update();
+
+  //getSerialData();
+  getFreqSerialData();
+  getAmpSerialData();
+
+}
+
+void updateDisplay(){
+  oled.clearField(fieldWidth, rowsPerLine * 2, 4);
+  oled.print(frequency);
+  oled.clearField(fieldWidth, rowsPerLine * 3, 4);
+  oled.print(amplitude);
+}
+
+bool showSerialData(void *) {
+
+  Serial.print("\nrpm/freq real: ");
+  Serial.print(frequency);
+  Serial.print("\t");
+  Serial.print(frequency / 60 * ports);
+
+  return true;
+}
+
+void getFreqSerialData() {
+  while (freqSerial.available()) {
+    Serial.print(char(freqSerial.read()));
+  }
+}
+
+void getAmpSerialData() {
+  while (ampSerial.available()) {
+    Serial.print(char(ampSerial.read()));
+  }
+}
+
+void getSerialData() {
+  if (Serial.available() > 0) {
+    // ignore line feed and carriage return
+    if (Serial.peek() == '\n' || Serial.peek() == '\r') {
+      Serial.read();  // read and discard LF & CR
+    } else {
+      frequency = Serial.parseFloat();
+      Serial.println(frequency, 4);  // print to 4 decimal places
+    }
+  }
+}

src/siren_remote_control_esp32/siren_remote_control_esp32.ino

@@ -0,0 +1,192 @@
+#include "WiFi.h"
+
+#include <Wire.h>
+
+// Display Libraries
+#include <Wire.h>
+#include <SSD1306Ascii.h>
+#include <SSD1306AsciiWire.h>
+
+// Other Libraries
+#include <arduino-timer.h>
+#include <AiEsp32RotaryEncoder.h>
+#include <ArduinoOSCWiFi.h>
+
+// WiFi stuff
+const char* ssid = "sirening";
+const char* pwd = "alarm_11735";
+const IPAddress ip(192, 168, 4, 200);
+const IPAddress gateway(192, 168, 4, 1);
+const IPAddress subnet(255, 255, 0, 0);
+
+// for ArduinoOSC
+const char* hosts[] = {"192.168.4.201", "192.168.4.202", "192.168.4.203"};
+const int settings_port = 54000;
+const int state_port = 54001;
+
+// Button Defs
+bool freqButtonState = false;
+AiEsp32RotaryEncoder freqEncoderButton = AiEsp32RotaryEncoder(34, 35, 32, -1, 4);
+bool ampButtonState = false;
+AiEsp32RotaryEncoder ampEncoderButton = AiEsp32RotaryEncoder(33, 25, 26, -1, 4);
+int sirenSelect = 0;
+
+//TODO: These should be bound to a publication of the frequency directly from the siren
+float frequencies[] = {0, 0, 0};
+float amplitudes[] = {0, 0, 0};
+const int ports = 5;
+
+// Display Defs
+#define I2C_ADDRESS 0x3C
+SSD1306AsciiWire oled;
+uint8_t fieldWidth;
+uint8_t rowsPerLine; // Rows per line.
+
+void killAll() {
+  for (int h = 0; h < 3; h++) {
+    OscWiFi.send(hosts[h], settings_port, "/kill");
+  }
+}
+
+float hzToRads(int freq){
+  return freq * TWO_PI / ports;
+}
+
+float rpmToRads(int rpm){
+  return (rpm / 60) * TWO_PI;
+}
+
+void setupOSC(char* path){
+  OscWiFi.subscribe(state_port, path, [](const OscMessage& m) {
+    int motorIndex =  m.arg<int>(0);
+    float frequency = m.arg<float>(1);
+    float amplitude = m.arg<float>(2);
+    if(frequency != frequencies[motorIndex]){
+      frequencies[motorIndex] = frequency;
+      freqEncoderButton.setEncoderValue(frequency * 10.0);
+      updateDisplay();
+    }
+    if(amplitude != amplitudes[motorIndex]){
+      amplitudes[motorIndex] = amplitude;
+      ampEncoderButton.setEncoderValue(amplitude * 10.0);
+      updateDisplay();
+    }
+  });
+}
+
+void IRAM_ATTR readFreqEncoderISR() {
+  freqEncoderButton.readEncoder_ISR();
+}
+
+void IRAM_ATTR readAmpEncoderISR() {
+  ampEncoderButton.readEncoder_ISR();
+}
+
+void setupEncoderButton(AiEsp32RotaryEncoder& eb, char* val){
+  eb.begin();
+  if(val == "freq") {
+    eb.setup(readFreqEncoderISR);
+  } else {
+    eb.setup(readAmpEncoderISR);
+  }
+  eb.setBoundaries(0, 4000, false);
+  eb.setAcceleration(1000);
+}
+
+void setup() {
+
+  // initialize serial communication at 115200 bits per second:
+  Serial.begin(115200); //used for debugging.
+  delay(2000);
+
+  WiFi.softAPConfig(ip, gateway, subnet);
+  WiFi.softAP(ssid, pwd);
+
+  Serial.println("");
+  Serial.print("WiFi AP IP = ");
+  Serial.println(WiFi.softAPIP());
+
+  setupOSC("/state");
+
+  setupEncoderButton(freqEncoderButton, "freq");
+  setupEncoderButton(ampEncoderButton, "amp");
+
+  Wire.begin();
+  oled.begin(&Adafruit128x64, I2C_ADDRESS);
+  //oled.setI2cClock(100000L);
+  oled.setFont(System5x7);
+  oled.clear();
+  oled.println();
+  oled.println();
+  oled.println("  siren:     0");
+  oled.println("  frequency: 0");
+  oled.print("  amplitude: 0");
+  fieldWidth = oled.fieldWidth(strlen("  amplitude: "));
+  rowsPerLine = oled.fontRows();
+}
+
+void encoderButtonUpdate(AiEsp32RotaryEncoder& eb, bool& buttonState, char* val) {
+  if (eb.encoderChanged()) {
+    float setting = eb.readEncoder() / 10.0;
+    OscWiFi.send(hosts[sirenSelect], settings_port, "/" + String(val), setting);
+    Serial.print("new " + String(val) + " is: ");
+    Serial.println(setting);
+    updateDisplay();
+  }
+
+  if (eb.isEncoderButtonDown() && !buttonState) {
+    buttonState = true;
+  }
+
+  //button is up
+  if (!eb.isEncoderButtonDown() && buttonState) {
+      if(val == "freq"){
+        sirenSelect = (sirenSelect + 1) % 3;
+      } else {
+        killAll();
+      }
+      updateDisplay();
+      buttonState = false;
+  }
+}
+
+void loop()
+{
+  encoderButtonUpdate(freqEncoderButton, freqButtonState, "freq");
+  encoderButtonUpdate(ampEncoderButton, ampButtonState, "amp");
+
+  OscWiFi.update();
+}
+
+void updateDisplay(){
+  oled.clearField(fieldWidth, rowsPerLine * 2, 4);
+  oled.print(sirenSelect + 1);
+  oled.clearField(fieldWidth, rowsPerLine * 3, 4);
+  oled.print(frequencies[sirenSelect]);
+  oled.clearField(fieldWidth, rowsPerLine * 4, 4);
+  oled.print(amplitudes[sirenSelect]);
+}
+
+bool showSerialData(void *) {
+
+  Serial.print("\nrpm/freq real: ");
+  Serial.print(frequencies[sirenSelect]);
+  Serial.print("\t");
+  Serial.print(frequencies[sirenSelect] / 60 * ports);
+
+  return true;
+}
+
+/*
+void getSerialData() {
+  if (Serial.available() > 0) {
+    // ignore line feed and carriage return
+    if (Serial.peek() == '\n' || Serial.peek() == '\r') {
+      Serial.read();  // read and discard LF & CR
+    } else {
+      frequency[sirenSelect] = Serial.parseFloat();
+      Serial.println(frequency[sirenSelect], 4);  // print to 4 decimal places
+    }
+  }
+}
+*/