yes there are many examples, but i can’t find “velocity” in them. they are angle and acc
exactly, i don’t want to invent smth. so i’m asking what did you use for your projects? care to share some codes?
Velocity? Do you mean wheel velocity (speed)?
But if you talk about angular velocity, it is only gyro value in degrees per second. Without acc.
My English is poor, maybe I not understand…
Yes gyro velocity.(degrees/second)
How do you extract that? I can’t access it.
How do you calculate this gyro speed?
Delta(angle)/ time?
Extract? From what? What method you use to read sensor data? I read raw values.
you are right, my sensor module gives “angle”.
but most of the gyro sensors, give angular velocity and acc. so get an integral of gyro values to get angle right?
now i get it. thanks for the replies
Can we discuss some mechanics there?
I want to know what better to use with high torque and high speed GT types belt or HTD types belt or my be trapezoidal types?
Does anybody know ?
Or may be have some experience with it ?
well as far as i remember there are some datasheets from manufacturers which will guide you through choosing the right size belt.
generally they have some diagrams which will tell you the type for your application regarding your speed and torque(thus your Power). in your project i don’t think the torque and speeds would be a mechanical challenge for the belts. so i think u probably can go with whatever fits your design better.
I dont know how can i share gif but I hope you can see it
1 Like
Hi community. Thank you for all those sharing on balancing robots.
I decided doing a first test with some parts in stock and you will find below the result. I just implemented the balancing and movement back and forth because I had only one motor available. It looks to work fine. However, I have some questions that make me embarrassed before going ahead on a more complex project.
For the test I used those parts: 1 x BGC3.1 MOSFET + the mpu6050 delivered with; 1 x HC-05 Bluetooth Module; 1 x 2804 100KV Brushless motor; 1 x AS5600 Magnetic Encoder used in analogic; 1 x LiPo battery 3S 11.1V. And tried to implement after adaptation the code from @Antun_Skuric Skuric that was made for a Stm32 nucleo. Because of low memory I had to use the library ver. 1.61 otherwise the sketch was too big and overloaded the dynamic memory too.
Sorry if it looks to you basic but my first question is to know if there is a way for implement the last library released without memory issue?
The second question is about the pendulum behavior. It is stabilizing quite well but there is no positioning loop for avoid drifting. This is visible on the second video when I push it doesn’t come back on its position. I thought the used of the magnetic sensor should help. What would be the reason? The library (voltage control / torque control) or should I add another control loop?
#include "imu_helpers.h"
#include "src/SimpleFOC.h" // ver. 1.61
// create motor and sensor instances
BLDCMotor motorL = BLDCMotor(3, 5, 6, 7);
MagneticSensorAnalog sensorL = MagneticSensorAnalog(A0, 14, 1020);
//BLDCMotor motorR = BLDCMotor(9, 10, 11, 7);
//MagneticSensorAnalog sensorR = MagneticSensorAnalog(A1, 14, 1020);
// control algorithm parameters
// stabilisation pid
PIDController pid_stb{.P = 50, .I = 90, .D = 2, .ramp = 100000, .limit = 7};
// velocity pid
PIDController pid_vel{.P = 0.005, .I = 0, .D = 0, .ramp = 1000, .limit = _PI / 10};
// velocity control filtering
LowPassFilter lpf_pitch_cmd{.Tf = 0.15};
// low pass filters for user commands - throttle and steering
LowPassFilter lpf_throttle{.Tf = 0.5};
LowPassFilter lpf_steering{.Tf = 0.4};
// Bluetooth app variables
float steering = 0;
float throttle = 0;
float max_throttle = 20; // 20 rad/s
float max_steering = 1; // 1 V
float MaxAngle = PI/8; // activation limit
int state = 1; // 1 on / 0 off
char Incoming_value = 0; // Variable for storing serial value
void setup() {
// monitoring port
Serial.begin(9600);
delay(1000);
// imu init and configure
if ( !initIMU() ) {
Serial.println(F("IMU connection problem... Disabling!"));
return;
}
delay(1000);
sensorL.init(); // initialise magnetic sensor hardware
motorL.linkSensor(&sensorL); // link the motor to the sensor
// sensorR.init(); // initialise magnetic sensor hardware
// motorR.linkSensor(&sensorR); // link the motor to the sensor
motorL.voltage_power_supply = 11;
motorL.controller = ControlType::voltage; // set control loop type to be used
motorL.foc_modulation = FOCModulationType::SpaceVectorPWM; // choose FOC modulation (optional)
motorL.init(); // initialize motor
motorL.initFOC(); // align encoder and start FOC
// motorR.voltage_power_supply = 11;
// motorR.controller = ControlType::voltage; // set control loop type to be used
// motorR.foc_modulation = FOCModulationType::SpaceVectorPWM; // choose FOC modulation (optional)
// motorR.init(); // initialize motor
// motorR.initFOC(); // align encoder and start FOC
delay(2000);
Serial.println(F("Balancing robot ready!"));
}
void loop() {
// iterative setting FOC phase voltage
motorL.loopFOC();
// motorR.loopFOC();
// iterative function setting the outter loop target
motorL.move();
// motorR.move();
if (!state) { // if balancer disabled
motorL.target = 0;
//motorR.target = 0;
} else if ( hasDataIMU() ) { // when IMU has received the package
// read pitch from the IMU
float pitch = getPitchIMU();
//Serial.println(pitch);
if (pitch < MaxAngle && pitch > -MaxAngle){
pitch = pitch - PI/180; //offset de verticale /72
// calculate the target angle for throttle control
// float target_pitch = lpf_pitch_cmd(pid_vel((motorL.shaft_velocity + motorR.shaft_velocity) / 2 - lpf_throttle(throttle)));
float target_pitch = lpf_pitch_cmd(pid_vel(motorL.shaft_velocity - lpf_throttle(throttle)));
// calculate the target voltage
float voltage_control = pid_stb(target_pitch - pitch);
// filter steering
float steering_adj = lpf_steering(steering);
// set the tergat voltage value
motorL.target = voltage_control + steering_adj;
// motorR.target = voltage_control - steering_adj;
}
else {
motorL.target = 0;
// motorR.target = 0;
}
}
// read the user command from bluetooth
Bluetooth();
}
/**
Send several time forward or backward command for accelerate
*/
void Bluetooth() {
if (Serial.available()> 0){
Incoming_value = Serial.read();
if(Incoming_value != '\n')
{
if (Incoming_value == '1') {
// ON Byte
// steering = 0;
throttle = 0;
state = 1;
//bt_port.println("State ON");
} else if (Incoming_value == '0') {
// OFF Byte
// steering = 0;
throttle = 0;
state = 0;
//bt_port.println("State OFF");
} else if (Incoming_value == 'A') {
if (throttle < 0)throttle = 0;
if (throttle < max_throttle) throttle = throttle + 5; // move forward
} else if (Incoming_value == 'R') {
if (throttle > 0)throttle = 0;
if (throttle > - max_throttle) throttle = throttle - 5; // move backward
}
else if (Incoming_value == 'S'){
// steering = 0;
throttle = 0;
}
}
}
}
2 Likes
Still don’t have find solution. any advice?
Hi @CREGIS21 ,
Thanks for sharing the details and videos of your project, that’s really cool 
For the memory problems, I unfortunately cannot give you an easy solution- the only way is to cut parts out of the program (especially like printing) to reduce the memory size.
Or else you switch to a different MCU/driver that has more memory…
For the control loop, you are running it in voltage mode with the balance loop, and can control it with Bluetooth. (Is that working?)
But I don’t see any control loop that would return the robot to a given encoder position? So I think it won’t do this unless you add some code to change the control from “throttle based” to positional control.
Hi @runger ,
Many thanks for the answer.
For the control loop it is a similar code than @Antun_Skuric developed. So I was thinking that would behave the same. But it is not the case.
GitHub Arduino SimpleFOCBalancer
Look at this video : Torture test - SimpleFOC-balancer
Hi,
It’s a good question! In Antun’s video, it does look like it is returning to a position… maybe it’s just chance it looks this way, or maybe Antun has a more complex control loop in that video.
Or maybe I am missing it in the code, but it looks to me like the balancer code is doing balancing, and has throttle control, but there is no control loop based on encoder position?
So it’s a mystery! Perhaps @Antun_Skuric can comment when he has time 
Hey @CREGIS21,
So in my video I am using the same controller as in the repo.
The controller has two parts, stabilization loop and the velocity control loop.
The stabilization loop is essentially controlled with the motors’ acceleration. So it on its own would not guarantee that the motors’ velocity would go back to 0. If you remove the velocity loop, the robot would be stable but it would probably never stop moving.
That’s why the velocity loop was added, to make the robot stop.
I’ve opted for the velocity loop in order to be able to control its speed with a joystick.
In my video that double (stabilization + velocity) loop is the one producing the behavior of leaning.
However, the robot does not return to its original position. The control loop only searches to make it stop moving, not to to go to the original location.
If you’d like it to come back to its original position, there are essentially two ways of doing so:
- Add another loop with low P gain (no D or I gains).
float target_velocity = P_gain*( 0 - motor.shaft_angle);
float target_pitch = lpf_pitch_cmd(pid_vel(target_velocity - motor.shaft_velocity));
You can start with small values of P_gain and raise it until you get the results you’re searching for.
- Do a state space controller instead of two/three loops:
voltage_control = k1*pendulum_angle +k2*pendulum_velocity +k3*motor.shaft_velocity +k4*motor.shaft_angle;
This controller would be able to stabilize the robot and keep it at the same place. Here is an old post that might help you find the good gains k1-k4.
1 Like
Many thanks @Antun_Skuric . Your advice are always of a great value. I will test it and let you know the results.
I did it just to test ESP32 with SimpleFOCMini drivers and MT6701 magnetic encoders.
9 Likes
Here is the result. It is the mascot of my robotic association on a kind of hoverboard. Done in 3D printing
1 Like
Oh no this is my mascot
As usual your robot is a please to watch!
It’s also a beautiful result! I hope you will get to show it off at many events!
1 Like
Hello guys. I found your constructions on YouTube and I’m amazed. I would like to build a self-balancing robot. Before buying the electronics, I would like to ask if what I have chosen makes sense. What do you think? Will it work?
- MKS DUAL FOC
- AS5600
- MPU6050
- 12N14P
Do you have any suggestions for me?