Change pins
This commit is contained in:
Lucien Renaud
parent
ba62b3987b
commit
77896cfc55
130
src/main.cpp
130
src/main.cpp
|
|
@ -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();
|
||||
}
|
||||
|
|
@ -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
|
||||
Loading…
Reference in New Issue