Skip to content

GitLab

Commit 28518533 authored by Barciś, Agata's avatar Barciś, Agata
Browse files

old Balancer removed

parent 3fa53b7a
Hide whitespace changes
Inline Side-by-side
Balancer/Balance.cpp deleted 100644 → 0
View file @ 3fa53b7a
#include <Wire.h>
#include "Balance.h"
int32_t gYZero;
int32_t angle; // millidegrees
int32_t angleRate; // degrees/s
int32_t distanceLeft;
int32_t speedLeft;
int32_t driveLeft;
int32_t distanceRight;
int32_t speedRight;
int32_t driveRight;
int16_t motorSpeed;
bool isBalancingStatus;
bool balanceUpdateDelayedStatus;
bool isBalancing()
{
return isBalancingStatus;
}
bool balanceUpdateDelayed()
{
return balanceUpdateDelayedStatus;
}
void balanceSetup()
{
// Initialize IMU.
Wire.begin();
if (!imu.init())
{
while(true)
{
Serial.println("Failed to detect and initialize IMU!");
delay(200);
}
}
imu.enableDefault();
imu.writeReg(LSM6::CTRL2_G, 0b01011000); // 208 Hz, 1000 deg/s
// Wait for IMU readings to stabilize.
delay(1000);
// Calibrate the gyro.
int32_t total = 0;
for (int i = 0; i < CALIBRATION_ITERATIONS; i++)
{
imu.read();
total += imu.g.y;
delay(1);
}
gYZero = total / CALIBRATION_ITERATIONS;
}
// This function contains the core algorithm for balancing a
// Balboa 32U4 robot.
void balance()
{
// Adjust toward angle=0 with timescale ~10s, to compensate for
// gyro drift. More advanced AHRS systems use the
// accelerometer as a reference for finding the zero angle, but
// this is a simpler technique: for a balancing robot, as long
// as it is balancing, we know that the angle must be zero on
// average, or we would fall over.
angle = angle * 999 / 1000;
// This variable measures how close we are to our basic
// balancing goal - being on a trajectory that would cause us
// to rise up to the vertical position with zero speed left at
// the top. This is similar to the fallingAngleOffset used
// for LED feedback and a calibration procedure discussed at
// the end of Balancer.ino.
//
// It is in units of millidegrees, like the angle variable, and
// you can think of it as an angular estimate of how far off we
// are from being balanced.
int32_t risingAngleOffset = angleRate * ANGLE_RATE_RATIO + angle;
// Combine risingAngleOffset with the distance and speed
// variables, using the calibration constants defined in
// Balance.h, to get our motor response. Rather than becoming
// the new motor speed setting, the response is an amount that
// is added to the motor speeds, since a *change* in speed is
// what causes the robot to tilt one way or the other.
motorSpeed += (
+ ANGLE_RESPONSE * risingAngleOffset
+ DISTANCE_RESPONSE * (distanceLeft + distanceRight)
+ SPEED_RESPONSE * (speedLeft + speedRight)
) / 100 / GEAR_RATIO;
if (motorSpeed > MOTOR_SPEED_LIMIT)
{
motorSpeed = MOTOR_SPEED_LIMIT;
}
if (motorSpeed < -MOTOR_SPEED_LIMIT)
{
motorSpeed = -MOTOR_SPEED_LIMIT;
}
// Adjust for differences in the left and right distances; this
// will prevent the robot from rotating as it rocks back and
// forth due to differences in the motors, and it allows the
// robot to perform controlled turns.
int16_t distanceDiff = distanceLeft - distanceRight;
motors.setSpeeds(
motorSpeed + distanceDiff * DISTANCE_DIFF_RESPONSE / 100,
motorSpeed - distanceDiff * DISTANCE_DIFF_RESPONSE / 100);
}
void lyingDown()
{
// Reset things so it doesn't go crazy.
motorSpeed = 0;
distanceLeft = 0;
distanceRight = 0;
motors.setSpeeds(0, 0);
if (angleRate > -2 && angleRate < 2)
{
// It's really calm, so we know the angles.
if (imu.a.z > 0)
{
angle = 80000;
}
else
{
angle = -80000;
}
distanceLeft = 0;
distanceRight = 0;
}
}
void integrateGyro()
{
// Convert from full-scale 1000 deg/s to deg/s.
angleRate = (imu.g.y - gYZero) / 29;
angle += angleRate * UPDATE_TIME_MS;
}
void integrateEncoders()
{
static int16_t lastCountsLeft;
int16_t countsLeft = encoders.getCountsLeft();
speedLeft = (countsLeft - lastCountsLeft);
distanceLeft += countsLeft - lastCountsLeft;
lastCountsLeft = countsLeft;
static int16_t lastCountsRight;
int16_t countsRight = encoders.getCountsRight();
speedRight = (countsRight - lastCountsRight);
distanceRight += countsRight - lastCountsRight;
lastCountsRight = countsRight;
}
void balanceDrive(int16_t leftSpeed, int16_t rightSpeed)
{
driveLeft = leftSpeed;
driveRight = rightSpeed;
}
void balanceDoDriveTicks()
{
distanceLeft -= driveLeft;
distanceRight -= driveRight;
speedLeft -= driveLeft;
speedRight -= driveRight;
}
void balanceResetEncoders()
{
distanceLeft = 0;
distanceRight = 0;
}
void balanceUpdateSensors()
{
imu.read();
integrateGyro();
integrateEncoders();
}
void balanceUpdate()
{
static uint16_t lastMillis;
uint16_t ms = millis();
// Perform the balance updates at 100 Hz.
if ((uint16_t)(ms - lastMillis) < UPDATE_TIME_MS) { return; }
balanceUpdateDelayedStatus = ms - lastMillis > UPDATE_TIME_MS + 1;
lastMillis = ms;
balanceUpdateSensors();
balanceDoDriveTicks();
if (imu.a.x < 3000)
{
lyingDown();
isBalancingStatus = false;
}
else
{
balance();
isBalancingStatus = true;
}
}
Balancer/Balance.h deleted 100644 → 0
View file @ 3fa53b7a
#pragma once
#include <stdint.h>
#include <LSM6.h>
#include <Balboa32U4.h>
// This code was developed for a Balboa unit using 50:1 motors
// and 45:21 plastic gears, for an overall gear ratio of 111.
// Adjust the ratio below to scale various constants in the
// balancing algorithm to match your robot.
//#define B11 false
#define B11 true
#if B11
const int16_t GEAR_RATIO = 124; //b11
#else
const int16_t GEAR_RATIO = 111; //b10
#endif
// This constant limits the maximum motor speed. If your gear
// ratio is lower than what we used, or if you are testing
// changes to the code, you might want to reduce it to prevent
// your robot from zooming away when things go wrong.
//
// If you want to use speeds faster than 300, you should add
// the line "motors.allowTurbo(true);" to setup().
const int16_t MOTOR_SPEED_LIMIT = 400;
// This constant relates the angle to its rate of change for a
// robot that is falling from a nearly-vertical position or
// rising up to that position. The relationship is nearly
// linear. For example, if you have the 80mm wheels it should be
// about 140, which means that the angle in millidegrees is ~140
// times its rate of change in degrees per second; when the robot
// has fallen by 90 degrees it will be moving at about
// 90,000/140 = 642 deg/s. See the end of Balancer.ino for one
// way to calibrate this value.
const int16_t ANGLE_RATE_RATIO = 140;
// The following three constants define a PID-like algorithm for
// balancing. Each one determines how much the motors will
// respond to the corresponding variable being off from zero.
// See the code in Balance.cpp for exactly how they are used. To
// get it balancing from scratch, start with them all at zero and
// adjust them as follows:
// ANGLE_RESPONSE determines the response to a combination of
// angle and angle_rate; the combination measures how far the
// robot is from a stable trajectory. To test this, use your
// hand to flick the robot up from a resting position. With a
// value that is too low, it won't stop itself in time; a high
// value will cause it to slam back into the ground or oscillate
// wildly back and forth. When ANGLE_RESPONSE is adjusted
// properly, the robot will move just enough to stop itself.
// However, after stopping itself, it will be moving and keep
// moving in the same direction, usually driving faster and
// faster until it reaches its maximum motor speed and falls
// over. That's where the next constants come in.
#if B11
const int16_t ANGLE_RESPONSE = 13; //35 //40
#else
const int16_t ANGLE_RESPONSE = 13; //35 //40
#endif
// DISTANCE_RESPONSE determines how much the robot resists being
// moved away from its starting point. Counterintuitively, this
// constant is positive: to move forwards, the robot actually has
// to first roll its wheels backwards, so that it can *fall*
// forwards. When this constant is adjusted properly, the robot
// will no longer zoom off in one direction, but it will drive
// back and forth a few times before falling down.
#if B11
const int16_t DISTANCE_RESPONSE = 110;
#else
const int16_t DISTANCE_RESPONSE = 70;
#endif
// DISTANCE_DIFF_RESPONSE determines the response to differences
// between the left and right motors, preventing undesired
// rotation due to differences in the motors and gearing. Unlike
// DISTANCE_REPONSE, it should be negative: if the left motor is
// lagging, we need to increase its speed and decrease the speed
// of the right motor. If this constant is too small, the robot
// will spin left and right as it rocks back and forth; if it is
// too large it will become unstable.
const int16_t DISTANCE_DIFF_RESPONSE = -50;
// SPEED_RESPONSE supresses the large back-and-forth oscillations
// caused by DISTANCE_RESPONSE. Increase this until these
// oscillations die down after a few cycles; but if you increase
// it too much it will tend to shudder or vibrate wildly.
#if B11
const int16_t SPEED_RESPONSE = 8000;
#else
const int16_t SPEED_RESPONSE = 4400;
#endif
// The balancing code is all based on a 100 Hz update rate; if
// you change this, you will have to adjust many other things.
const uint8_t UPDATE_TIME_MS = 5;
// Take 100 measurements initially to calibrate the gyro.
const uint8_t CALIBRATION_ITERATIONS = 100;
// These variables will be accessible from your sketch.
extern int32_t angle; // units: millidegrees
extern int32_t angleRate; // units: degrees/s (or millidegrees/ms)
extern int16_t motorSpeed; // current (average) motor speed setting
// These variables must be defined in your sketch.
extern LSM6 imu;
extern Balboa32U4Motors motors;
extern Balboa32U4Encoders encoders;
// Call this in your setup() to initialize and calibrate the IMU.
void balanceSetup();
// Call this in loop() to run the full balancing algorithm.
void balanceUpdate();
// Call this function to set a driving speed in ticks/ms. The
// way it works is that every update cycle we adjust the robot's
// encoder measurements, which will cause it to drive in the
// corresponding direction. Differing values for left and right
// will result in a turn.
void balanceDrive(int16_t leftSpeed, int16_t rightSpeed);
// Returns true if (according to the balancing algorithm) the
// robot is trying to balance. When it falls down it shuts off
// the motors, and this function will return false. If you pick
// the robot up, this function will start returning true again.
bool isBalancing();
// Returns true if the last update cycle was delayed to more than
// UPDATE_TIME_MS+1 milliseconds. This could indicate
// computations being too long or interrupts that are delaying
// the loop.
bool balanceUpdateDelayed();
// Sometimes you will want to take control of the motors but keep
// updating the balancing code's encoders and angle measurements
// so you don't lose track of the robot's position and angle.
// Call this every 10ms (UPDATE_TIME_MS) to update the sensors,
// and you will be able to resume balancing immediately when you
// are done.
void balanceUpdateSensors();
// Call this function to reset the encoders. This is useful
// after a large motion, so that robot does not try to make a
// huge correction to get back to "zero".
void balanceResetEncoders();
Balancer/Balancer.ino deleted 100644 → 0
View file @ 3fa53b7a
// This example shows how to make a Balboa balance on its two
// wheels and drive around while balancing.
//
// To run this demo, you will need to install the LSM6 library:
//
// https://github.com/pololu/lsm6-arduino
//
// To use this demo, place the robot on the ground with the
// circuit board facing up, and then turn it on. Be careful to
// not move the robot for a few seconds after powering it on,
// because that is when the gyro is calibrated. During the gyro
// calibration, the red LED is lit. After the red LED turns off,
// turn the robot so that it is standing up. It will detect that
// you have turned it and start balancing.
//
// Alternatively, you can press the A button while the robot is
// lying down and it will try to use its motors to kick up into
// the balancing position.
//
// This demo is tuned for the 50:1 high-power gearmotor with
// carbon brushes, 45:21 plastic gears, and 80mm wheels; you will
// need to adjust the parameters in Balance.h for your robot.
//
// After you have gotten the robot balance well, you can
// uncomment some lines in loop() to make it drive around and
// play a song.
#include <Balboa32U4.h>
#include <Wire.h>
#include <LSM6.h>
#include "Balance.h"
LSM6 imu;
Balboa32U4Motors motors;
Balboa32U4Encoders encoders;
Balboa32U4Buzzer buzzer;
Balboa32U4ButtonA buttonA;
Balboa32U4ButtonB buttonB;
bool musicOn = false;
void setup()
{
// Uncomment these lines if your motors are reversed.
// motors.flipLeftMotor(true);
// motors.flipRightMotor(true);
motors.allowTurbo(true);
ledYellow(0);
ledRed(1);
balanceSetup();
ledRed(0);
}
const char song[] PROGMEM =
"!O6 T240"
"l32ab-b>cl8r br b-bb-a a-r gr g-4 g4"
"a-r gr g-gg-f er e-r d4 e-4"
"gr msd8d8ml d-4d4"
"l32efg-gl8r msd8d8ml d-4d4"
"<bcd-d e-efg- ga-ab- a4 gr";
void playSong()
{
if (!buzzer.isPlaying())
{
buzzer.playFromProgramSpace(song);
}
}
void driveAround()
{
uint16_t time = millis() % 8192;
uint16_t leftSpeed, rightSpeed;
if (time < 1900)
{
leftSpeed = 20;
rightSpeed = 20;
}
else if (time < 4096)
{
leftSpeed = 25;
rightSpeed = 15;
}
else if (time < 4096 + 1900)
{
leftSpeed = 20;
rightSpeed = 20;
}
else
{
leftSpeed = 15;
rightSpeed = 25;
}
balanceDrive(leftSpeed/8, rightSpeed/8);
}
void standUp()
{
motors.setSpeeds(0, 0);
buzzer.play("!>grms>a16>a16>g2");
ledGreen(1);
ledRed(1);
ledYellow(1);
while (buzzer.isPlaying());
motors.setSpeeds(-MOTOR_SPEED_LIMIT, -MOTOR_SPEED_LIMIT);
delay(200);
motors.setSpeeds(310, 310); //MOTOR_SPEED_LIMIT, MOTOR_SPEED_LIMIT);
for (uint8_t i = 0; i < 30; i++)
{
delay(UPDATE_TIME_MS);
balanceUpdateSensors();
Serial.println(angle);
if(angle < 60000)
{
break;
}
}
motorSpeed = MOTOR_SPEED_LIMIT; //150;
balanceResetEncoders();
}
void loop()
{
balanceUpdate();
if (isBalancing() && musicOn) {
// Once you have it balancing well, uncomment these lines for
// something fun.
playSong();
driveAround();
} else {
buzzer.stopPlaying();
if (buttonA.getSingleDebouncedPress())
{
musicOn = false;
standUp();
balanceDrive(0, 0);
}
if (buttonB.getSingleDebouncedPress())
{
musicOn = true;
standUp();
}
}
// Illuminate the red LED if the last full update was too slow.
ledRed(balanceUpdateDelayed());
// Display feedback on the yellow and green LEDs depending on
// the variable fallingAngleOffset. This variable is similar
// to the risingAngleOffset used in Balance.cpp.
//
// When the robot is rising toward vertical (not falling),
// angleRate and angle have opposite signs, so this variable
// will just be positive or negative depending on which side of
// vertical it is on.
//
// When the robot is falling, the variable measures how far off
// it is from a trajectory starting it almost perfectly
// balanced then falling to one side or the other with the
// motors off.
//
// Since this depends on ANGLE_RATE_RATIO, it is useful for
// calibration. If you have changed the wheels or added weight
// to your robot, you can try checking these items, with the
// motor power OFF (powered by USB):
//
// 1. Try letting the robot fall with the Balboa 32U4 PCB up.
// The green LED should remain lit the entire time. If it
// sometimes shows yellow instead of green, reduce
// ANGLE_RATE_RATIO.
//
// 2. If it is tilted beyond vertical and given a push back to
// the PCB-up side again, the yellow LED should remain lit
// until it hits the ground. If you see green, increase
// ANGLE_RATE_RATIO.
//
// In practice, it is hard to achieve both 1 and 2 perfectly,
// but if you can get close, your constant will probably be
// good enough for balancing.
int32_t fallingAngleOffset = angleRate * ANGLE_RATE_RATIO - angle;
if (fallingAngleOffset > 0)
{
ledYellow(1);
ledGreen(0);
}
else
{
ledYellow(0);
ledGreen(1);
}
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment