Change pins

This commit is contained in:
Lucien Renaud 2022-08-23 13:45:39 +02:00
parent ba62b3987b
commit 77896cfc55
2 changed files with 104 additions and 55 deletions

View File

@ -1,20 +1,21 @@
#include <Arduino.h>
#include <SimpleFOC.h>
#include <linearHallSensor.h>
#include <pinout.h>
BLDCMotor motor = BLDCMotor(4);
BLDCDriver3PWM driver = BLDCDriver3PWM(7, 8, 9);
BLDCDriver3PWM driver = BLDCDriver3PWM(M2_PWM1, M2_PWM2, M2_PWM3);
LinearHallSensor linearSensor = LinearHallSensor(A0, A1, -0.75, 2.35);
LinearHallSensor linearSensor = LinearHallSensor(M2_Hall1, M2_Hall2, -0.75, 2.35);
void initSensor()
{
linearSensor.init(motor);
linearSensor.init(motor);
}
float callback()
{
return linearSensor.readSensorCallback();
return linearSensor.readSensorCallback();
}
GenericSensor sensor = GenericSensor(callback, initSensor);
@ -26,73 +27,92 @@ float target = 0.0;
void serialLoop()
{
static String received_chars;
while (Serial.available())
{
char inChar = (char)Serial.read();
received_chars += inChar;
if (inChar == '\n')
static String received_chars;
while (Serial.available())
{
target = received_chars.toFloat();
Serial.print("Target = ");
Serial.print(target);
received_chars = "";
char inChar = (char)Serial.read();
received_chars += inChar;
if (inChar == '\n')
{
target = received_chars.toFloat();
Serial.print("Target = ");
Serial.print(target);
received_chars = "";
}
}
}
}
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
}
void setup()
{
Serial.begin(115200);
_delay(100);
Serial.println("INIT");
pinMode(3, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
digitalWrite(3, LOW);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
driver.voltage_power_supply = 6.0;
driver.init();
motor.linkDriver(&driver);
motor.useMonitoring(Serial);
motor.controller = MotionControlType::angle;
motor.foc_modulation = FOCModulationType::SinePWM;
motor.voltage_limit = 1.5;
motor.voltage_sensor_align = 0.8;
motor.PID_velocity.P = 0.075f;
motor.PID_velocity.I = 0.01;
motor.PID_velocity.D = 0.0;
motor.LPF_velocity.Tf = 0.01f;
motor.P_angle.P = 120.0;
motor.P_angle.I = 10.0;
motor.velocity_limit = 50;
Serial.begin(115200);
delay(2000);
Serial.println("INIT");
motor.useMonitoring(Serial);
// pinMode(M1_PWM1, OUTPUT);
// pinMode(M1_PWM2, OUTPUT);
// pinMode(M1_PWM3, OUTPUT);
// digitalWrite(M1_PWM1, LOW);
// digitalWrite(M1_PWM2, LOW);
// digitalWrite(M1_PWM3, LOW);
// Init sensor
motor.init();
Serial.println("calibrating sensor in open loop...");
sensor.init();
Serial.println("Done");
delay(1000);
// pinMode(M2_PWM1, OUTPUT);
// pinMode(M2_PWM2, OUTPUT);
// pinMode(M2_PWM3, OUTPUT);
// digitalWrite(M2_PWM1, LOW);
// digitalWrite(M2_PWM2, LOW);
// digitalWrite(M2_PWM3, LOW);
motor.linkSensor(&sensor);
motor.init();
// motor.initFOC(5.48, CCW);
motor.initFOC();
// pinMode(M3_PWM1, OUTPUT);
// pinMode(M3_PWM2, OUTPUT);
// pinMode(M3_PWM3, OUTPUT);
// digitalWrite(M3_PWM1, LOW);
// digitalWrite(M3_PWM2, LOW);
// digitalWrite(M3_PWM3, LOW);
pinMode(LED_BUILTIN, OUTPUT); // Lightup LED
digitalWrite(LED_BUILTIN, HIGH);
driver.voltage_power_supply = 5.0;
driver.init();
motor.linkDriver(&driver);
motor.useMonitoring(Serial);
motor.controller = MotionControlType::angle;
motor.foc_modulation = FOCModulationType::SinePWM;
motor.voltage_limit = 1.5;
motor.voltage_sensor_align = 0.8;
motor.PID_velocity.P = 0.075f;
motor.PID_velocity.I = 0.01;
motor.PID_velocity.D = 0.0;
motor.LPF_velocity.Tf = 0.01f;
motor.P_angle.P = 120.0;
motor.P_angle.I = 10.0;
motor.velocity_limit = 50;
motor.useMonitoring(Serial);
// Init sensor
motor.init();
Serial.println("calibrating sensor in open loop...");
sensor.init();
Serial.println("Done");
delay(1000);
motor.linkSensor(&sensor);
motor.init();
// motor.initFOC(5.48, CCW);
motor.initFOC();
}
void loop()
{
serialLoop();
motor.move(target);
motor.loopFOC();
motor.monitor();
serialLoop();
motor.move(target);
motor.loopFOC();
motor.monitor();
}

29
src/pinout.h Normal file
View File

@ -0,0 +1,29 @@
#ifndef pinout_h
#define pinout_h
#include "Arduino.h"
// PCB board : STM32 DJI Gimbal V1.0
#define M1_PWM1 PB13
#define M1_PWM2 PB14
#define M1_PWM3 PB15
#define M1_Fault PB12
#define M1_Hall1 PB1
#define M1_Hall2 PB0
#define M2_PWM1 PB9
#define M2_PWM2 PB8
#define M2_PWM3 PB5
#define M2_Fault PB10
#define M2_Hall1 PA4
#define M2_Hall2 PA3
#define M3_PWM1 PA5
#define M3_PWM2 PA6
#define M3_PWM3 PA7
#define M3_Fault PA0
#define M3_Hall1 PA2
#define M3_Hall2 PA1
#endif