【创意智造组】当创客遇上概率论：用FOC电机和二哈2打造智能色子概率统计装置

#include <Arduino.h>
#include <Wire.h>
#include <SimpleFOC.h>
#include <WiFi.h>
#include <PubSubClient.h>

// ==================== 网络配置 ====================
const char* ssid = "sxs";
const char* password = "smj080823";
const char* mqtt_server = "192.168.31.11";
const int mqtt_port = 1883;
const char* mqtt_user = "siot";
const char* mqtt_password = "dfrobot";

// ==================== MQTT 主题 ====================
const char* control_topic = "siot/camera";          // 接收拍照指令（可选保留）
const char* status_topic = "siot/motor";     // 发送电机状态的主题

WiFiClient espClient;
PubSubClient client(espClient);

// ==================== 电机控制部分 ====================
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(5, 13, 10, 2);

float target_torque = 0.0f;
const float FORWARD_VOLTAGE = 4.0f;
const float REVERSE_VOLTAGE = -4.0f;
const float STOP_VOLTAGE = 0.0f;

const unsigned long DIRECTION_DURATION = 500;
const unsigned long STOP_DURATION = 5000;

enum ShakeStateType {
  SHAKE_FORWARD,
  SHAKE_REVERSE,
  SHAKE_STOP
};

ShakeStateType shakeState = SHAKE_FORWARD;
unsigned long stateStartTime = 0;
int shakeCounter = 0;
const int TOTAL_SHAKES = 3;

// 标志：防止在 STOP 状态重复发送 MQTT 消息
bool mqttStopSent = false;


// ==================== MQTT 回调函数（接收指令） ====================
void callback(char* topic, byte* payload, unsigned int length) {
  String message;
  for (int i = 0; i < length; i++) {
    message += (char)payload[i];
  }
  Serial.print("Message arrived on topic: ");
  Serial.print(topic);
  Serial.print(". Message: ");
  Serial.println(message);

 
}

// ==================== 重连 MQTT 服务器 ====================
void reconnect() {
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    if (client.connect("ESP32_Camera_Controller", mqtt_user, mqtt_password)) {
      Serial.println("connected");
      client.subscribe(control_topic);
      Serial.print("Subscribed to topic: ");
      Serial.println(control_topic);
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      delay(5000);
    }
  }
}

// ==================== 发送 MQTT 状态消息 ====================
void sendMotorStatus(const char* status) {
  if (client.connected()) {
    client.publish(status_topic, status);
    Serial.print("MQTT published: ");
    Serial.print(status_topic);
    Serial.print(" -> ");
    Serial.println(status);
  } else {
    Serial.println("MQTT not connected, cannot send status");
  }
}

// ==================== 初始化 ====================
void setup() {
  Serial.begin(115200);
  pinMode(HUSKYLENS_A_BUTTON_PIN, OUTPUT);
  digitalWrite(HUSKYLENS_A_BUTTON_PIN, HIGH);

  // 连接WiFi
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("\nWiFi connected");

  // 配置 MQTT
  client.setServer(mqtt_server, mqtt_port);
  client.setCallback(callback);

  // 初始化电机
  sensor.init();
  motor.linkSensor(&sensor);
  driver.voltage_power_supply = 12;
  driver.voltage_limit = 6;
  driver.init();
  motor.linkDriver(&driver);
  motor.phase_resistance = 2.9;
  motor.torque_controller = TorqueControlType::voltage;
  motor.controller = MotionControlType::torque;
  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
  motor.init();
  motor.initFOC();

  shakeState = SHAKE_FORWARD;
  stateStartTime = millis();
  target_torque = FORWARD_VOLTAGE;
  motor.move(target_torque);
  shakeCounter = 0;
  mqttStopSent = false;
}

void loop() {
  // 维护 MQTT 连接
  if (!client.connected()) {
    reconnect();
  }
  client.loop();

  // 电机 FOC 更新
  motor.loopFOC();
  motor.move(target_torque);

  // ========== 电机状态机 ==========
  unsigned long now = millis();
  switch (shakeState) {
    case SHAKE_FORWARD:
      if (now - stateStartTime >= DIRECTION_DURATION) {
        shakeState = SHAKE_REVERSE;
        stateStartTime = now;
        target_torque = REVERSE_VOLTAGE;
        Serial.println("Forward done -> Reverse");
      }
      break;

    case SHAKE_REVERSE:
      if (now - stateStartTime >= DIRECTION_DURATION) {
        shakeCounter++;
        if (shakeCounter >= TOTAL_SHAKES) {
          shakeState = SHAKE_STOP;
          stateStartTime = now;
          target_torque = STOP_VOLTAGE;
          mqttStopSent = false;          // 重置发送标志
          Serial.println("All shakes done -> Stop 5s");
        } else {
          shakeState = SHAKE_FORWARD;
          stateStartTime = now;
          target_torque = FORWARD_VOLTAGE;
          Serial.print("Shake cycle ");
          Serial.print(shakeCounter);
          Serial.println(" completed, continue forward");
        }
      }
      break;

    case SHAKE_STOP:
      // 在 STOP 状态期间发送一次 MQTT 消息（例如进入停止后 0.5 秒发送）
      if (!mqttStopSent && (now - stateStartTime >= 500)) {
        sendMotorStatus("MOTOR_STOPPED");   // 可改为其他内容，如 "TAKE_PHOTO"
        mqttStopSent = true;
      }
      if (now - stateStartTime >= STOP_DURATION) {
        shakeCounter = 0;
        shakeState = SHAKE_FORWARD;
        stateStartTime = now;
        target_torque = FORWARD_VOLTAGE;
        Serial.println("Stop done -> Start new shake sequence");
      }
      break;
  }
  // ========== 状态机结束 ==========
}