NJUPT第一次积分赛

NJUPT第一次积分赛

最近在忙第二次积分赛以及一些很复杂的队友关系（人际关系好复杂，好想电赛出个单机模式），但最后结果还是很满意的。

突然想起来第一次积分赛写的屎山，遂拿出来给大火闻闻

没啥很新颖的东西，都是找一堆开源然后缝合的，所以感觉开源也没啥关系，拿出来以便后人参考。

主控和遥控器部分采用的都是esp32，主控esp32跑的RTOS，一个核用来跑FOC算法，另一个用来跑lvgl以及其他的一些东西。代码如下：

#include <Arduino.h>
#include <SimpleFOC.h>
#include <Wire.h>
#include <TFT_eSPI.h>
#include <AiEsp32RotaryEncoder.h>
#include <lvgl.h>
#include <WiFi.h>
#include <esp_now.h>
#include <stdio.h>
#include <stdlib.h>

#include "esp_freertos_hooks.h"
#include "esp_netif.h"
#include "esp_eth.h"
#include "esp_event.h"
#include "esp_log.h"
#include "esp_err.h"
#include "esp_system.h"
#include "esp_spi_flash.h"

#include <soc/soc.h>
#include <soc/rtc_cntl_reg.h>

#define ROTARY_ENCODER1_A_PIN 4
#define ROTARY_ENCODER1_B_PIN 16
#define ROTARY_ENCODER1_BUTTON_PIN 17
#define LV_TICK_PERIOD_MS				1
#define Press_KeyUP digitalRead(15)==LOW
#define Press_KeyDOWN digitalRead(19)==LOW
#define Press_KeyLEFT digitalRead(35)==LOW
#define Press_KeyRIGHT digitalRead(32)==LOW
#define Press_KeyCENTER digitalRead(34)==LOW

typedef struct _MPU6050_
{
  String boardName;
  int touch;
  int L_theta;
  int R_theta;
  int pitch;
  int roll;
  int yaw;
}_mpu;

//跑双核，core0(Task1)用来跑FOC算法，core1(Task2)用来通信与控制,core1(Task3)用来显示gui
void Task1( void *pvParameters );
void Task2( void *pvParameters );

_mpu mpuData;
AiEsp32RotaryEncoder Encoder1 = AiEsp32RotaryEncoder(ROTARY_ENCODER1_A_PIN, ROTARY_ENCODER1_B_PIN, ROTARY_ENCODER1_BUTTON_PIN, -1);
volatile int16_t encoderButton = 0;   //切换串口与旋钮控制
int16_t encoderValue = 0;
int16_t encoderAngle = 0;
int16_t encoderMulti = 0;
float target = 0;
volatile int flag_mode = 0;           //切换模式
int wifiConnectionStatus = 0;         //wifi连接状态
volatile int clientConnectionStatus = 0;       //客户端连接状态
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C); //编码器为AS5600，用iic通信
BLDCMotor motor = BLDCMotor(7);//电机极对数为7
BLDCDriver3PWM driver = BLDCDriver3PWM(33,25,26,27);//控制电机转动的三相pwm输出口分别为33,25,26，其中27是Enable口
Commander command = Commander(Serial);  

int shaft_angle;                      // current motor angle
int shaft_velocity;                   // current motor velocity
volatile int target_velocity = 10;    // current target velocity
volatile float target_angle = 0;      // current target angle
volatile float target_angle_MPU;
volatile int target_velocity_MPU;

TFT_eSPI tft = TFT_eSPI(240,240);      //TFT Object
static lv_disp_draw_buf_t draw_buf;    //定义显示器变量
static lv_color_t buf[TFT_WIDTH * 10]; //定义刷新缓存
lv_obj_t * Data = NULL;
lv_timer_t * timer = NULL;

static void lv_tick_task(void *arg) {(void) arg;lv_tick_inc(LV_TICK_PERIOD_MS);}
void doTarget(char* cmd){ command.scalar(&target, cmd);}
void onMotor(char*cmd){ command.motor(&motor,cmd); }
void tft_init();
int BLDC_init();
void Encoder_Init();
void lvgl_port_init();
void my_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, lv_color_t *color_p);
void timer_1cb(lv_timer_t * timer);
void WiFi_connect(const char *SSID, const char *Password);
void WiFi_Init();
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len);
void Key_init();

void setup()
{
  Serial.begin(115200);

  WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);//关闭低电压检测,避免无限重启
  xTaskCreatePinnedToCore(Task1, "Task1", 10000, NULL, 3, NULL,  0);  //最后一个参数至关重要，决定这个任务创建在哪个核上.PRO_CPU 为 0, APP_CPU 为 1,或者 tskNO_AFFINITY 允许任务在两者上运行.
  xTaskCreatePinnedToCore(Task2, "Task2", 10000, NULL, 1, NULL,  1);
}
void loop() {}

/*--------------------------------------------------------------------------------------------------------------------------*/
/*--------------------------------------------------下面是任务循环-----------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------------*/

void Task1(void *pvParameters) //跑FOC
{
  BLDC_init();
  vTaskDelay(1000);
  while(1)
  {
    switch (flag_mode)
    {
      case 0:if(encoderButton) target = target_velocity;break;
      case 1:if(encoderButton) target = target_angle;break;
      case 2:target = target_angle;break;
      case 3:target = target_velocity;break;
      case 4:target = target_velocity;break;
      case 5:target = target_velocity;break;
    }
    motor.loopFOC();
    motor.move(target);
  }
} 

/*--------------------------------------------------------------------------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------------*/

void Task2(void *pvParameters) //干杂活
{
  Encoder_Init();
  Key_init();
  WiFi_Init();
  //上位机指令
  command.add('T',doTarget,"target value");
  command.add('M',onMotor,"my motor");
  motor.useMonitoring(Serial);

  tft_init();
  lvgl_port_init();
  Data = lv_label_create(lv_scr_act());
  timer = lv_timer_create(timer_1cb, 50, 0);

  const esp_timer_create_args_t periodic_timer_args = {
		.callback = &lv_tick_task,          // 超时回调函数
		.name = "periodic_gui"              // 定时器名称
	};
  esp_timer_handle_t periodic_timer;                                                      // 定义定时器
	ESP_ERROR_CHECK(esp_timer_create(&periodic_timer_args, &periodic_timer));               // 创建定时器
	ESP_ERROR_CHECK(esp_timer_start_periodic(periodic_timer, LV_TICK_PERIOD_MS * 1000));    // 开始定时器，每LV_TICK_PERIOD_MS * 1000微妙触发一次

  if (esp_now_init() != 0)
  {
    Serial.println("Error initializing ESP-NOW");
  }
  esp_now_register_recv_cb(OnDataRecv);
  vTaskDelay(100);
  while(1)
  {
    command.run();
    motor.monitor();
    lv_timer_handler();
    vTaskDelay(5);
    switch(flag_mode)
    {
      case 0: motor.controller = MotionControlType::velocity;;break;
      case 1: motor.controller = MotionControlType::angle;break;
      case 2: motor.controller = MotionControlType::angle;break;      //MPU--ANGLE
      case 3: motor.controller = MotionControlType::velocity;break;   //MPU--VELOCITY
      case 4: motor.controller = MotionControlType::velocity;break;
      case 5: motor.controller = MotionControlType::velocity;break;
    }
    encoderValue = Encoder1.readEncoder();
    encoderAngle = encoderValue * 9 % 360;
    if (Encoder1.isEncoderButtonClicked())
    {
      static unsigned long lastTimePressed = 0;
      if (millis() - lastTimePressed < 500){}
      else
      {
        encoderButton = ~encoderButton;
      }
      lastTimePressed = millis();
    }
    if(flag_mode == 0)
    {
      target_velocity = encoderValue;
    }
    if(flag_mode == 1)
    {
      target_angle = encoderValue * 9.0 / 360 * 6.28 ;
    }
    if(flag_mode == 2)
    {
      target_angle = -motor.shaftAngle() - (mpuData.yaw) / 360.0 * 6.28;
    }
    if(flag_mode == 3)
    {
      target_velocity = -mpuData.pitch / 3.0;
    }
    if(flag_mode == 4)
    {
      if(mpuData.L_theta)
        target_velocity = 20;
      else
        target_velocity = 0;
      if(mpuData.R_theta)
        target_velocity = -20;
      else
        target_velocity = 0;
    }
    if(flag_mode == 5)
    {
      target_velocity = mpuData.roll / 3;
    }
    if(Press_KeyRIGHT)
    {
      vTaskDelay(180);
      flag_mode++;
    }
    if(flag_mode > 5) flag_mode = 0;
    if(flag_mode < 0) flag_mode = 5;
  }
}

/*--------------------------------------------------------------------------------------------------------------------------*/
/*-----------------------------------------------------下面是函数定义--------------------------------------------------------*/
/*--------------------------------------------------------------------------------------------------------------------------*/

void tft_init()
{
  tft.init();
  tft.fillScreen(TFT_BLACK);
  tft.setRotation(1);
  tft.setTextSize(2);
  tft.setTextColor(TFT_WHITE);
  tft.setCursor(0,0);
}
int BLDC_init()
{
  Wire.setPins(21,22);  //将GPIO21、GPIO22分别设置为SDA、SCL口
  Wire.begin();
  sensor.init(&Wire);
  motor.linkSensor(&sensor);
  driver.voltage_power_supply = 12; //12V供电
  driver.init();
  motor.linkDriver(&driver);
  motor.foc_modulation = FOCModulationType::SpaceVectorPWM; //FOC调制类型为SVPWM
  //PID参数
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 1.5;
  motor.PID_velocity.D = 0.005;
  motor.P_angle.P = 5.2;
  motor.P_angle.I = 0.026;
  motor.P_angle.D = 0.0005;

  motor.voltage_limit = 6;
  motor.LPF_velocity.Tf = 0.06; //对速度低通滤波
  motor.velocity_limit = 40;  //速度上限
  motor.voltage_sensor_align = 1;
  motor.init();
  motor.initFOC();
  return 1;
}
void Encoder_Init()
{
  Encoder1.begin();
  Encoder1.setup([]{Encoder1.readEncoder_ISR();});
  Encoder1.disableAcceleration();
  Encoder1.setBoundaries(-2000, 2000, false);//一圈最多39
}
void lvgl_port_init()
{
  lv_init();
  lv_disp_draw_buf_init(&draw_buf, buf, NULL, TFT_WIDTH * 10);
  static lv_disp_drv_t disp_drv;
  lv_disp_drv_init(&disp_drv);
  disp_drv.hor_res = TFT_WIDTH;
  disp_drv.ver_res = TFT_HEIGHT;
  disp_drv.flush_cb = my_disp_flush;
  disp_drv.draw_buf = &draw_buf;
  lv_disp_drv_register(&disp_drv);
}
void my_disp_flush(lv_disp_drv_t *disp, const lv_area_t *area, lv_color_t *color_p)
{
    uint32_t w = (area->x2 - area->x1 + 1);
    uint32_t h = (area->y2 - area->y1 + 1);

    tft.startWrite();                                        //使能写功能
    tft.setAddrWindow(area->x1, area->y1, w, h);             //设置填充区域
    tft.pushColors((uint16_t *)&color_p->full, w * h, true); //写入颜色缓存和缓存大小
    tft.endWrite();                                          //关闭写功能
    lv_disp_flush_ready(disp);                               //调用区域填充颜色函数
}

void timer_1cb(lv_timer_t * timer)
{
  shaft_angle = fabs((int)(motor.shaftAngle()*360/6.28) % 360);
  shaft_velocity = (int)motor.shaftVelocity();
  lv_label_set_text_fmt(Data,"EncoderButton: %d\nEncoderValue: %d\nRotaryAngle: %d\nMotorShaftAngle: %d\nMotorShaftVelocity: %d\nmode: %d\nClient Connection: %d\npitch: %d\nroll: %d\nyaw: %d\n JoyStick: %d\ntouch: %d\n",encoderButton,encoderValue,encoderAngle, shaft_angle, shaft_velocity, flag_mode%6, clientConnectionStatus, mpuData.pitch, mpuData.roll, mpuData.yaw,mpuData.L_theta / 6.28 * 360,mpuData.touch);
}

void WiFi_connect(const char *SSID, const char *Password)
{
  WiFi.begin(SSID,Password);
  Serial.print("WIFI Connecting");
  while (WiFi.status()!=WL_CONNECTED)
  {
    delay(1000);
    Serial.print(".");
  }
  Serial.println();
  Serial.println("WiFi Connection Established!");
  wifiConnectionStatus = 1;
  Serial.print("IP Address:"); Serial.println(WiFi.localIP());
}
void WiFi_Init()
{
  WiFi.mode(WIFI_STA);
  WiFi.setSleep(false);
}
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len)
{
  memcpy(&mpuData, incomingData, sizeof(mpuData));
  clientConnectionStatus = 1;
  Serial.print("Board name:"); Serial.println(mpuData.boardName);
}
void Key_init()
{
  pinMode(15,INPUT_PULLUP);
  pinMode(19,INPUT_PULLUP);
  pinMode(32,INPUT_PULLUP);
  pinMode(35,INPUT_PULLUP);
  pinMode(34,INPUT_PULLUP);
}

下面是遥控部分。两块esp32采用esp_now通信，摇杆部分有点bug，不会搞，但最后还是蒙混过去了。

#include <Arduino.h>
#include <WiFi.h>
#include <Wire.h>
#include <MPU6050_tockn.h>
#include <esp_now.h>

#define tm T6

typedef struct _MPU6050_
{
  String boardname;
  int touch;
  int L_theta;
  int R_theta;
  int pitch;
  int roll;
  int yaw;
}_mpu;

const int threshold=48;
uint8_t broadcastAddress[] = {0x08, 0xB6, 0x1F, 0x32, 0xFE, 0xE8}; //MAC地址
String sendJSONData;
_mpu mpuData;
MPU6050 mpu6050(Wire);
int JoyStickL_X;
int JoyStickL_Y;
int JoyStickR_X;
int JoyStickR_Y;
int tc;

long timer = 0;

void WiFi_connect(const char *SSID, const char *Password);
void WiFi_Init();
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status); // 数据发送回调函数

void setup()
{
  Serial.begin(115200);
  WiFi_Init();
  if (esp_now_init() != ESP_OK)
  {
    Serial.println("Error initializing ESP-NOW");
  }
  mpuData.touch = 0;
  esp_now_register_send_cb(OnDataSent); //设置发送数据回调函数
  esp_now_peer_info_t peerInfo = {};
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;
  if (esp_now_add_peer(&peerInfo) != ESP_OK)
  {
    Serial.println("Failed to add peer");
  }       

  pinMode(2,OUTPUT);
  digitalWrite(2,LOW);
  Wire.begin(21,22);
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);

}

void loop()
{
  mpu6050.update();
  mpuData.boardname = "MyESP_1";
  mpuData.pitch = (int)mpu6050.getAngleY();
  mpuData.roll = (int)mpu6050.getAngleX();
  mpuData.yaw = (int)mpu6050.getAngleZ();
  JoyStickL_X = analogRead(35);
  JoyStickL_Y = (int)analogRead(34);
  JoyStickR_X = analogRead(33);
  JoyStickR_Y = analogRead(32);
  tc=touchRead(tm);
  if(tc < threshold)
  {
    mpuData.touch = 0;
  }
  else mpuData.touch = 1;
  if(JoyStickL_Y > 4070)
    mpuData.L_theta = 1;
  else if(JoyStickL_Y < 20)
    mpuData.L_theta = -1;
  else
    mpuData.L_theta = 0;

  if(JoyStickR_Y < 20)
    mpuData.R_theta = 1;
  else if(JoyStickR_Y > 4090)
    mpuData.R_theta = -1;
  else
    mpuData.R_theta = 0;
  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &mpuData, sizeof(mpuData));
  if (result == ESP_OK)
  {
    Serial.println("Sent with success");
  }
  else
  {
    Serial.println("Error sending the data");
  }
  Serial.print(JoyStickR_Y); Serial.print(" ");Serial.println(JoyStickL_Y);
  Serial.println(mpuData.touch);
  delay(10);
}

void WiFi_connect(const char *SSID, const char *Password)
{
  WiFi.begin(SSID,Password);
  Serial.print("WIFI Connecting");
  while (WiFi.status()!=WL_CONNECTED)
  {
    delay(1000);
    Serial.print(".");
  }
  Serial.println();
  Serial.println("WiFi Connection Established!");
  Serial.print("IP Address:"); Serial.println(WiFi.localIP());
}
void WiFi_Init()
{
  WiFi.mode(WIFI_STA);
  WiFi.setSleep(false);
}
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status)
{
  char macStr[18];
  Serial.print("Packet to: ");
  snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  Serial.println(macStr);
  Serial.print("Send status: ");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
  Serial.println();
}

就这么多吧，等后面忙完了把第二次积分赛的图像部分放出来。