Hi, I am quite new with electronics, programming and arduino. I'm working on this project:
One of the issues I'm having is that I have only an UNO board, I read that it changes nothing with a NANO (which is the board used in the original project). Apart from the original code, the author made some custom to control the accelerometers and stepper motor, do I have to chenge the to UNO also?
Please extract wiring and code and post it here. Penetrating projects and/or videos calls for too much time for many helpers.How to get the best out of this forum - Using Arduino / IDE 1.x - Arduino Forum
The only major difference between the Nano and UNO is that the Nano has two additional analog input pins (A6 and A7) that are not present on the UNO. Other than that, the code should run without modification.
1 Like
Here's the main code.
/*
* This is the controller code for the reaction wheel satellite model published
* at https://charleslabs.fr/en/project-Reaction+Wheel+Attitude+Control
*
* The electronics diagram and the mechanical build is described in this article.
*
* You can set the PID controller terms and various other parameters hereunder.
*
* Charles GRASSIN, 2021
* MIT License
*/
#define SERIAL_DEBUG_ENABLE 1 /* 0 = disable, 1 = enable */
#define MPU6050_CALIBRATION 0 /* 0 = disable, 1 = enable */
#define CONTROLLER_MODE 0 /* 0 = Speed stabilization only, 1 = Speed and Attitude stabilization, 2 = same as 1, but change set point every N secondes */
// -------PINS-------
#define PIN_DIR 2
#define PIN_STEP 3
#define PIN_SLEEP 4
#define PIN_LED 13
// ------------------
// -----STEPPER------
#include "AccelStepper.h" // This is a hacked version of this library to allow
// speed control instead of position control.
AccelStepper myStepper(1, PIN_STEP, PIN_DIR);
double motorSpeed = 0;
#define MICROSTEPPING 4 /* 1 or 2 or 4 or 8 or 16 or 32 */
#define ACCELERATION 1750 /* Steps per s */
#define MAX_SPEED (600 * MICROSTEPPING)
// ------------------
// -------PID--------
#include "PID.h"
const double P_TERM = 0.050 * MICROSTEPPING;
const double I_TERM = 0.000 * MICROSTEPPING;
const double D_TERM = 0.017 * MICROSTEPPING;
PIDController pidSpeed(P_TERM, I_TERM, D_TERM);
PIDAngleController pidAttitude(2.5, 0, 400);
// ------------------
// ------MPU6050-----
#include <Wire.h>
#define MPU_ADDRESS 0x68 // I2C address of the MPU-6050
double GYRO_ERROR = 61.96; // rollingAvg error compensation
double yawAngle=0, yawAngularSpeed=0;
// ------------------
// ------GENERAL-----
long timeCur, timePrev, timeStart;
const int numReadings= 5;
double readings[numReadings];
int readIndex = 0;
double total = 0, rollingAvg = 0;
double targetAttitude = 0;
// ------------------
void setup() {
#if SERIAL_DEBUG_ENABLE == 1
Serial.begin(115200);
delay(500);
Serial.println("Attitude Speed");
#endif
// Gyro setup
Wire.begin();
Wire.beginTransmission(MPU_ADDRESS);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // wakes up the MPU-6050
Wire.endTransmission(true);
Wire.beginTransmission(MPU_ADDRESS);
Wire.write(0x1B); // GYRO_CONFIG register
Wire.write(0x10); // 1000dps full scale
Wire.endTransmission(true);
// Gyro cal (only if selected, otherwise use saved value)
#if MPU6050_CALIBRATION == 1
calibrateMPU();
#endif
// LED
pinMode(PIN_LED,OUTPUT);
digitalWrite(PIN_LED,1);
// Initial stepper parameters
myStepper.setEnablePin (PIN_SLEEP);
myStepper.setAcceleration(ACCELERATION);
setSpeedStepper(0);
timeCur = millis();
timeStart = timeCur;
}
// FSM variables
byte controllerState = 0;
int counts = 0;
// Main loop
void loop() {
// Stop control after 40s
// if(millis() - timeStart > 40000){
// digitalWrite(PIN_LED,0);
// myStepper.disableOutputs ();
// }
// Pulse stepper
myStepper.run();
// Every 10ms, read MPU and call controllers
if(millis() - timeCur > 10) {
timePrev = timeCur;
timeCur = millis();
// Measure Gyro value
yawAngularSpeed = ((double)readMPU()-GYRO_ERROR) / 32.8;
yawAngle += (yawAngularSpeed * (timeCur - timePrev) / 1000.0);
// Put angle between -180 and 180
while (yawAngle <= -180) yawAngle += 360;
while (yawAngle > 180) yawAngle -= 360;
// Low Pass Filter the angular speed (https://www.arduino.cc/en/Tutorial/BuiltInExamples/Smoothing)
total = total - readings[readIndex];
readings[readIndex] = yawAngularSpeed;
total = total + readings[readIndex];
readIndex = readIndex + 1;
if (readIndex >= numReadings)
readIndex = 0;
rollingAvg = total / numReadings;
// Compute controller output
#if CONTROLLER_MODE == 0
// Detumbling only
motorSpeed += pidSpeed.compute(0,rollingAvg,timeCur - timePrev);
#else if CONTROLLER_MODE == 1 || CONTROLLER_MODE == 2
// Change set point
#if CONTROLLER_MODE == 2
counts ++;
if(counts == 250) {
counts = 0;
if(targetAttitude == 0)
targetAttitude = 180;
else
targetAttitude = 0;
}
#endif
// FSM transition
if(controllerState == 1 && fabs(rollingAvg) > 360 /* °/s */){
controllerState = 0;digitalWrite(PIN_LED,0);
}
else if(controllerState == 0 && fabs(rollingAvg) < 45 /* °/s */)
controllerState = 1;
//FSM action
if(controllerState == 0){
motorSpeed += pidSpeed.compute(0,rollingAvg,timeCur - timePrev);
pidAttitude.compute(targetAttitude,yawAngle,timeCur - timePrev);
}
else
motorSpeed += pidSpeed.compute(pidAttitude.compute(targetAttitude,yawAngle,timeCur - timePrev),rollingAvg,timeCur - timePrev);
#endif
// Constrain speed to valid interval (saturation)
if(motorSpeed > MAX_SPEED) motorSpeed = MAX_SPEED;
else if (motorSpeed < -MAX_SPEED) motorSpeed = -MAX_SPEED;
setSpeedStepper(motorSpeed);
// Report attitude and speed
#if SERIAL_DEBUG_ENABLE == 1
Serial.print(yawAngle);
Serial.print(" ");
Serial.println(rollingAvg);
#endif
}
}
// Set the current speed and direction of the motor
void setSpeedStepper(double targetSpeed){
if(targetSpeed > 0)
myStepper.moveTo(1000000);
else
myStepper.moveTo(-1000000);
myStepper.setMaxSpeed(fabs(targetSpeed));
}
// Read a yaw angular speed value
int16_t readMPU(){
Wire.beginTransmission(MPU_ADDRESS);
Wire.write(0x47);
Wire.endTransmission(false);
Wire.requestFrom(MPU_ADDRESS,2,true);
return Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
}
// Calibrate the gyro by doing CALIBRATION_MEASUREMENTS_COUNT measurements
#define CALIBRATION_MEASUREMENTS_COUNT 200
void calibrateMPU(){
GYRO_ERROR = 0;
for(int i=0;i<CALIBRATION_MEASUREMENTS_COUNT;i++){
GYRO_ERROR += readMPU();
delay(20);
}
GYRO_ERROR = GYRO_ERROR/(double)CALIBRATION_MEASUREMENTS_COUNT;
#if SERIAL_DEBUG_ENABLE == 1
Serial.println(GYRO_ERROR);
#endif
}
The author's code corrects for gyro offset error, and that correction won't be the same for your sensor. You probably need to calibrate the gyro, by setting this option in the code.
#define MPU6050_CALIBRATION 0 /* 0 = disable, 1 = enable */
Once you have enabled that, the calibration takes place in setup(). Do not move the sensor until the calibration offset is printed, then edit the program to correct that value.
How do I do that? I mean, I tried changing to 1 (enable) the calibration, but sincerely I don't know how to do it. In the monitor serie I don't get any offset. Also, if I have this offset, where do I write it in the code?
Thank you.
Post the revised code. The program should have printed the offset.
It is not a great idea to start off with an advanced project like this one. At some point, you will have to learn how to read and make sensible changes to programs written for Arduino.
We recommend to start much smaller, like learning how to blink an LED, read a switch or a temperature, control a simple DC motor, etc. There are plenty of examples built in to the Arduino IDE, and countless tutorials posted on the web.
The purpose of this project is to pass "Control Systems" subject. I am not completely new in Arduino, I have already made a 24x6 matrix LED during high school (long time ago). Here's the code, I changed the pins. In the Serial Print, Attitude and Speed read 0, even if I move the gyro.
/*
* This is the controller code for the reaction wheel satellite model published
* at https://charleslabs.fr/en/project-Reaction+Wheel+Attitude+Control
*
* The electronics diagram and the mechanical build is described in this article.
*
* You can set the PID controller terms and various other parameters hereunder.
*
* Charles GRASSIN, 2021
* MIT License
*/
#define SERIAL_DEBUG_ENABLE 1 /* 0 = disable, 1 = enable */
#define MPU6050_CALIBRATION 1 /* 0 = disable, 1 = enable */
#define CONTROLLER_MODE 0 /* 0 = Speed stabilization only, 1 = Speed and Attitude stabilization, 2 = same as 1, but change set point every N secondes */
// -------PINS-------
#define PIN_DIR 8 // Change to pin 8 on Arduino Uno
#define PIN_STEP 9 // Change to pin 9 on Arduino Uno
#define PIN_SLEEP 10 // Change to pin 10 on Arduino Uno
#define PIN_LED 13
// ------------------
// -----STEPPER------
#include "AccelStepper.h"
AccelStepper myStepper(1, PIN_STEP, PIN_DIR); // Use pins 9 (STEP) and 8 (DIR) on Arduino Uno
double motorSpeed = 0;
#define MICROSTEPPING 4
#define ACCELERATION 1750
#define MAX_SPEED (600 * MICROSTEPPING)
// ------------------
// -------PID--------
#include "PID.h"
const double P_TERM = 0.5 * MICROSTEPPING;
const double I_TERM = 0.000 * MICROSTEPPING;
const double D_TERM = 0.05 * MICROSTEPPING;
PIDController pidSpeed(P_TERM, I_TERM, D_TERM);
PIDAngleController pidAttitude(2.5, 0, 400);
// ------------------
// ------MPU6050-----
#include<Wire.h>
#define MPU_ADDRESS 0x69
double GYRO_ERROR = 61.96;
double yawAngle = 0, yawAngularSpeed = 0;
// ------------------
// ------GENERAL-----
long timeCur, timePrev, timeStart;
const int numReadings= 5;
double readings[numReadings];
int readIndex = 0;
double total = 0, rollingAvg = 0;
double targetAttitude = 0;
// ------------------
void setup() {
#if SERIAL_DEBUG_ENABLE == 1
Serial.begin(115200);
delay(500);
Serial.println("Attitude Speed");
#endif
// Gyro setup
Wire.begin();
Wire.beginTransmission(MPU_ADDRESS);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // wakes up the MPU-6050
Wire.endTransmission(true);
Wire.beginTransmission(MPU_ADDRESS);
Wire.write(0x1B); // GYRO_CONFIG register
Wire.write(0x10); // 1000dps full scale
Wire.endTransmission(true);
// Gyro cal (only if selected, otherwise use saved value)
#if MPU6050_CALIBRATION == 1
calibrateMPU();
#endif
// LED
pinMode(PIN_LED,OUTPUT);
digitalWrite(PIN_LED,1);
// Initial stepper parameters
myStepper.setEnablePin (PIN_SLEEP);
myStepper.setAcceleration(ACCELERATION);
setSpeedStepper(0);
timeCur = millis();
timeStart = timeCur;
}
// FSM variables
byte controllerState = 0;
int counts = 0;
// Main loop
void loop() {
// Stop control after 40s
// if(millis() - timeStart > 40000){
// digitalWrite(PIN_LED,0);
// myStepper.disableOutputs ();
// }
// Pulse stepper
myStepper.run();
// Every 10ms, read MPU and call controllers
if(millis() - timeCur > 10) {
timePrev = timeCur;
timeCur = millis();
// Measure Gyro value
yawAngularSpeed = ((double)readMPU()-GYRO_ERROR) / 32.8;
yawAngle += (yawAngularSpeed * (timeCur - timePrev) / 1000.0);
// Put angle between -180 and 180
while (yawAngle <= -180) yawAngle += 360;
while (yawAngle > 180) yawAngle -= 360;
// Low Pass Filter the angular speed (https://www.arduino.cc/en/Tutorial/BuiltInExamples/Smoothing)
total = total - readings[readIndex];
readings[readIndex] = yawAngularSpeed;
total = total + readings[readIndex];
readIndex = readIndex + 1;
if (readIndex >= numReadings)
readIndex = 0;
rollingAvg = total / numReadings;
// Compute controller output
#if CONTROLLER_MODE == 0
// Detumbling only
motorSpeed += pidSpeed.compute(0,rollingAvg,timeCur - timePrev);
#else if CONTROLLER_MODE == 1 || CONTROLLER_MODE == 2
// Change set point
#if CONTROLLER_MODE == 2
counts ++;
if(counts == 250) {
counts = 0;
if(targetAttitude == 0)
targetAttitude = 180;
else
targetAttitude = 0;
}
#endif
// FSM transition
if(controllerState == 1 && fabs(rollingAvg) > 360 /* °/s */){
controllerState = 0;digitalWrite(PIN_LED,0);
}
else if(controllerState == 0 && fabs(rollingAvg) < 45 /* °/s */)
controllerState = 1;
//FSM action
if(controllerState == 0){
motorSpeed += pidSpeed.compute(0,rollingAvg,timeCur - timePrev);
pidAttitude.compute(targetAttitude,yawAngle,timeCur - timePrev);
}
else
motorSpeed += pidSpeed.compute(pidAttitude.compute(targetAttitude,yawAngle,timeCur - timePrev),rollingAvg,timeCur - timePrev);
#endif
// Constrain speed to valid interval (saturation)
if(motorSpeed > MAX_SPEED) motorSpeed = MAX_SPEED;
else if (motorSpeed < -MAX_SPEED) motorSpeed = -MAX_SPEED;
setSpeedStepper(motorSpeed);
// Report attitude and speed
#if SERIAL_DEBUG_ENABLE == 1
Serial.print(yawAngle);
Serial.print(" ");
Serial.println(rollingAvg);
#endif
}
}
// Set the current speed and direction of the motor
void setSpeedStepper(double targetSpeed){
if(targetSpeed > 0)
myStepper.moveTo(1000000);
else
myStepper.moveTo(-1000000);
myStepper.setMaxSpeed(fabs(targetSpeed));
}
// Read a yaw angular speed value
int16_t readMPU(){
Wire.beginTransmission(MPU_ADDRESS);
Wire.write(0x47);
Wire.endTransmission(false);
Wire.requestFrom(MPU_ADDRESS,2,true);
return Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
}
// Calibrate the gyro by doing CALIBRATION_MEASUREMENTS_COUNT measurements
#define CALIBRATION_MEASUREMENTS_COUNT 200
void calibrateMPU(){
GYRO_ERROR = 0;
for(int i=0;i<CALIBRATION_MEASUREMENTS_COUNT;i++){
GYRO_ERROR += readMPU();
delay(20);
}
GYRO_ERROR = GYRO_ERROR/(double)CALIBRATION_MEASUREMENTS_COUNT;
#if SERIAL_DEBUG_ENABLE == 1
Serial.println(GYRO_ERROR);
#endif
}
The IMU is not working. Either you have wired it incorrectly, or you are using the wrong I2C address, or, given that the MPU-6050 was discontinued years ago, you have a nonfunctional counterfeit or clone.
The first step in this project would be to verify that you have a working IMU. Use your favorite search engine, together with the phrase "arduino mpu6050" to get started.
system
June 21, 2024, 2:15am
10
This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.
