main_genericSTM32F405RG.cpp

#include <Arduino.h>
#include <Wire.h>
#include <SimpleFOC.h>
/*****
 * I'm using a 16Mhz HSE oscillator therefore don't forget to set -D HSE_VALUE=16000000U as build flag
 */


void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 8;
  RCC_OscInitStruct.PLL.PLLN = 168;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
  
  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
  {
    Error_Handler();
  }
}

// void SystemClock_Config(void)
// {
//   RCC_OscInitTypeDef RCC_OscInitStruct = {0};
//   RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

//   /** Configure the main internal regulator output voltage
//   */
//   __HAL_RCC_PWR_CLK_ENABLE();
//   __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
//   /** Initializes the RCC Oscillators according to the specified parameters
//   * in the RCC_OscInitTypeDef structure.
//   */
//   RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
//   RCC_OscInitStruct.HSIState = RCC_HSI_ON;
//   RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
//   RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
//   RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
//   RCC_OscInitStruct.PLL.PLLM = 8;
//   RCC_OscInitStruct.PLL.PLLN = 168;
//   RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
//   RCC_OscInitStruct.PLL.PLLQ = 4;
//   if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
//   {
//     Error_Handler();
//   }
//   /** Initializes the CPU, AHB and APB buses clocks
//   */
//   RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
//                               |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
//   RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
//   RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
//   RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
//   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

//   if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
//   {
//     Error_Handler();
//   }
// }

#define SENSE_U1  PA0
#define SENSE_V1  PA1
#define SENSE_W1  PA15

#define SENSE_U2  PC0
#define SENSE_V2  PC1
#define SENSE_W2  PC2

#define HALL_U1  PA2
#define HALL_V1  PA3
#define HALL_W1  PC5

#define HALL_U2  PB10
#define HALL_V2  PB11
#define HALL_W2  PC9

#define IN_U1     PA8
#define IN_V1     PA9
#define IN_W1     PA10

#define IN_U2     PC6
#define IN_V2     PC7
#define IN_W2     PC8

#define EN_U1     PA7
#define EN_V1     PB0
#define EN_W1     PB1

#define EN_U2     PA5
#define EN_V2     PB14
#define EN_W2     PB15

#define SPI_SCLK  PC10
#define SPI_MISO  PC11
#define SPI_MOSI  PC12
#define SPI_SS1 PC13
#define SPI_SS2 PC14

#define I2C1_SCL PB8
#define I2C1_SDA PB9


#define LED_BLUE PB4
#define LED_GREEN PB5

#define foo LED_BUILTIN

// HardwareSerial Serial1(PB7, PB6);

HallSensor sensor = HallSensor(HALL_U1, HALL_V1, HALL_W1, 15);
BLDCDriver3PWM driver = BLDCDriver3PWM(IN_U1, IN_V1, IN_W1);
BLDCMotor motor = BLDCMotor(15);

// Interrupt routine intialisation
// channel A and B callbacks
void doA(){sensor.handleA();}
void doB(){sensor.handleB();}
void doC(){sensor.handleC();}

float target=1.0;

void serialReceiveUserCommand() {
  
  // a string to hold incoming data
  static String received_chars;
  
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the string buffer:
    received_chars += inChar;
    // end of user input
    if (inChar == '\n') {
      
      // change the motor target
      target= received_chars.toFloat();
      Serial.print("Target: ");
      Serial.println(target);
      
      // reset the command buffer 
      received_chars = "";
    }
  }
}

void scan () {

  byte error, address;
  int nDevices;

  Serial.println("Scanning...");
  nDevices = 0;
  for(address = 1; address < 127; address++ ) 
  {
    // The i2c_scanner uses the return value of
    // the Write.endTransmisstion to see if
    // a device did acknowledge to the address.
    Wire.beginTransmission(address);
    error = Wire.endTransmission();

    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");

      nDevices++;
    }
    else if (error==4) 
    {
      Serial.print("Unknow error at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else
    Serial.println("done\n");

  delay(5000);      
}

float angle1 = 0;

void sector(int sector) {
  Serial.print(angle1); Serial.print("\t");
  Serial.println(sector);
}

void setup() {

    Serial.begin(1000000);
  delay(1000);
  Serial.println("Setup");
  pinMode(I2C1_SDA, INPUT_PULLUP);
  pinMode(I2C1_SCL, INPUT_PULLUP);
  Wire.setSDA(I2C1_SDA);
  Wire.setSCL(I2C1_SCL);
  Wire.setClock(400000);
  Wire.begin();

  pinMode(EN_U1, OUTPUT);
  pinMode(EN_V1, OUTPUT);
  pinMode(EN_W1, OUTPUT);
  pinMode(SENSE_U1, INPUT);
  pinMode(SENSE_V1, INPUT);
  pinMode(SENSE_W1, INPUT);

  digitalWrite(EN_U1, HIGH);
  digitalWrite(EN_V1, HIGH);
  digitalWrite(EN_W1, HIGH);


  delay(100);

 sensor.pullup = Pullup::EXTERN;
  
  // initialise encoder hardware
  sensor.init();
  // sensor.attachSectorCallback(&sector);
  // hardware interrupt enable
  sensor.enableInterrupts(doA, doB, doC);


  motor.voltage_limit = 2;
  motor.voltage_sensor_align = 0.75;
  driver.voltage_limit = 2;
  driver.voltage_power_supply = 12;
  driver.init();
  // link the motor and the driver
  motor.linkDriver(&driver);
  motor.linkSensor(&sensor);
  motor.useMonitoring(Serial);
  motor.LPF_velocity = 0.1;
  motor.PID_velocity.P = 0.8;
  motor.PID_velocity.I = 2;

  // limiting motor movements
  motor.velocity_limit = 20; // [rad/s]
  motor.foc_modulation = FOCModulationType::SinePWM;

  // open loop control config
  motor.controller = ControlType::velocity;

  // init motor hardware
  motor.init();
  motor.initFOC(2.5,Direction::CW);

  delay(1000);

  motor.move(1);



//   // check if you need internal pullups
 
//   pinMode(LED_BLUE, OUTPUT);
//   pinMode(LED_GREEN, OUTPUT);
//   // pinMode(PC_13, OUTPUT);
//   // pinMode(PC14, OUTPUT);

//   digitalWrite(PC_13, HIGH);
//   digitalWrite(PC14, HIGH);
  

//   digitalWrite(LED_BLUE, HIGH);
//   digitalWrite(LED_GREEN, HIGH);
//   delay(1000);
}

void loop() {
  // static int led_state = 0;
  // // delay(1000);
  // // HAL_Delay(100);

  // static float angle1 = 0;

  angle1 += 0.00002;

  // motor.setPhaseVoltage(1.5, 0, _electricalAngle(angle1,15));
  motor.move(target);
  motor.loopFOC();

  static long last = millis();
  long now = millis();
  if(now - last > 20) {
    // Serial.print(angle1); Serial.print("\t");
    // motor.monitor();
    // int u1 = analogRead(SENSE_U1);
    // int v1 = analogRead(SENSE_V1);
    // // int w1 = analogRead(SENSE_W1);
    // Serial.print(u1); Serial.print("\t");
    // Serial.print(v1); Serial.print("\t");
    // Serial.print(w1); Serial.print("\t");
  // Serial.print("\t");
  // Serial.println(sensor.getVelocity());
  // last = now;
  }

// scan();

  serialReceiveUserCommand();
}