So just to begin with I'm pretty new to Arduino but know just enough to get me into trouble. What I am trying to build is a Digital Angle Display for a lapidary machine with 90 being horizontal and 0 being vertical.
The hardware I have no problem figuring out but coding or even finding code that exists is where I've fallen short.
I've found some existing code but most is reversed from what I need or is only 1 deg increment and I need to the 1/10th display.
Does anyone know of a project out there that may fit the bill?
Perhaps you should mention how you intend to measure the angle.
For informed help, please read and follow the directions in the "How to get the best out of the forum", at the head of every topic listing.
trying to add an angle display to a lapidary machine ( used to cut gem stones)
You should post your goal and hardware (model numbers etc).
For instance:
- display in degrees of angle i.e 00.0 or 00.00
- Accuracy of measurement in degrees of angle
- Resolution of measurement.
- What Arduino you are using.
looking for 00.00, have a few different boards, uno r3, mega, adafruit huzzah. sensor are mpu 6250. MPU 9250 have been using an I2C 16x2 lcd for the display but have a few OLED displays,
0 to 90 in the pitch axis in .01 degree increments. Roll axis isn't needed but welcome -10 to 10 deg in .01 increments.
I've been working off this code from dronebot workshop so far and I'm close but not quite there.
Had to change the display const. due to the library used is no longer valid or updated.
My displayed angle is off by 90 deg and that it where I'm stuck so far along with adding a calibrate "0" and store calibration so it's needed on every power cycle.
Not sure if that's enough info just thankful for the help.
It may help to see a Lapidary with one on it ( UltraTec V5 )
/*
Electronic Level
mpu-6050-level.ino
Uses MPU-6050 IMU
Displays on 128x64 OLED and LED
DroneBot Workshop 2019
https://dronebotworkshop.com
*/
// Include Wire Library for I2C
#include <Wire.h>
// Include NewLiquidCrystal Library for I2C
#include <LiquidCrystal_I2C.h>
// Define LCD pinout
// const int en = 2, rw = 1, rs = 0, d4 = 4, d5 = 5, d6 = 6, d7 = 7, bl = 3;
// Define I2C Address - change if reqiuired
const int i2c_addr = 0x27;
LiquidCrystal_I2C lcd(i2c_addr, 16, 2);
// Level LEDs
int levelLED_neg1 = 9;
int levelLED_neg0 = 10;
int levelLED_level = 11;
int levelLED_pos0 = 12;
int levelLED_pos1 = 13;
//Variables for Gyroscope
int gyro_x, gyro_y, gyro_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
boolean set_gyro_angles;
long acc_x, acc_y, acc_z, acc_total_vector;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
float angle_pitch_output, angle_roll_output;
// Setup timers and temp variables
long loop_timer;
int temp;
// Display counter
int displaycount = 0;
void setup() {
//Start I2C
Wire.begin();
// Set display type as 16 char, 2 rows
lcd.begin(16,2);
// Set Level LEDs as outputs
pinMode(levelLED_neg1, OUTPUT);
pinMode(levelLED_neg0, OUTPUT);
pinMode(levelLED_level, OUTPUT);
pinMode(levelLED_pos0, OUTPUT);
pinMode(levelLED_pos1, OUTPUT);
//Setup the registers of the MPU-6050
setup_mpu_6050_registers();
//Read the raw acc and gyro data from the MPU-6050 1000 times
for (int cal_int = 0; cal_int < 1000 ; cal_int ++){
read_mpu_6050_data();
//Add the gyro x offset to the gyro_x_cal variable
gyro_x_cal += gyro_x;
//Add the gyro y offset to the gyro_y_cal variable
gyro_y_cal += gyro_y;
//Add the gyro z offset to the gyro_z_cal variable
gyro_z_cal += gyro_z;
//Delay 3us to have 250Hz for-loop
delay(3);
}
// Divide all results by 1000 to get average offset
gyro_x_cal /= 1000;
gyro_y_cal /= 1000;
gyro_z_cal /= 1000;
// Start Serial Monitor
Serial.begin(115200);
// Init Timer
loop_timer = micros();
}
void loop(){
// Get data from MPU-6050
read_mpu_6050_data();
//Subtract the offset values from the raw gyro values
gyro_x -= gyro_x_cal;
gyro_y -= gyro_y_cal;
gyro_z -= gyro_z_cal;
//Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)
//Calculate the traveled pitch angle and add this to the angle_pitch variable
angle_pitch += gyro_x * 0.0000611;
//Calculate the traveled roll angle and add this to the angle_roll variable
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
angle_roll += gyro_y * 0.0000611;
//If the IMU has yawed transfer the roll angle to the pitch angle
angle_pitch += angle_roll * sin(gyro_z * 0.000001066);
//If the IMU has yawed transfer the pitch angle to the roll angle
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);
//Accelerometer angle calculations
//Calculate the total accelerometer vector
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z));
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
//Calculate the pitch angle
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296;
//Calculate the roll angle
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296;
//Accelerometer calibration value for pitch
angle_pitch_acc -= 0.0;
//Accelerometer calibration value for roll
angle_roll_acc -= 0.0;
if(set_gyro_angles){
//If the IMU has been running
//Correct the drift of the gyro pitch angle with the accelerometer pitch angle
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;
//Correct the drift of the gyro roll angle with the accelerometer roll angle
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;
}
else{
//IMU has just started
//Set the gyro pitch angle equal to the accelerometer pitch angle
angle_pitch = angle_pitch_acc;
//Set the gyro roll angle equal to the accelerometer roll angle
angle_roll = angle_roll_acc;
//Set the IMU started flag
set_gyro_angles = true;
}
//To dampen the pitch and roll angles a complementary filter is used
//Take 90% of the output pitch value and add 10% of the raw pitch value
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;
//Take 90% of the output roll value and add 10% of the raw roll value
angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;
//Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
// Print to Serial Monitor
//Serial.print(" | Angle = "); Serial.println(angle_pitch_output);
// Increment the display counter
displaycount = displaycount +1;
if (displaycount > 100) {
lcd.clear();
// Print on first row of LCD
lcd.setCursor(0,0);
lcd.print("Pitch: ");
lcd.print(angle_pitch_output);
lcd.setCursor(0,1);
lcd.print("Roll: ");
lcd.print(angle_roll_output);
// Check Angle for Level LEDs
if (angle_pitch_output < -2.01) {
// Turn on Level LED
digitalWrite(levelLED_neg1, HIGH);
digitalWrite(levelLED_neg0, LOW);
digitalWrite(levelLED_level, LOW);
digitalWrite(levelLED_pos0, LOW);
digitalWrite(levelLED_pos1, LOW);
} else if ((angle_pitch_output > -2.00) && (angle_pitch_output < -1.01)) {
// Turn on Level LED
digitalWrite(levelLED_neg1, LOW);
digitalWrite(levelLED_neg0, HIGH);
digitalWrite(levelLED_level, LOW);
digitalWrite(levelLED_pos0, LOW);
digitalWrite(levelLED_pos1, LOW);
} else if ((angle_pitch_output < 1.00) && (angle_pitch_output > -1.00)) {
// Turn on Level LED
digitalWrite(levelLED_neg1, LOW);
digitalWrite(levelLED_neg0, LOW);
digitalWrite(levelLED_level, HIGH);
digitalWrite(levelLED_pos0, LOW);
digitalWrite(levelLED_pos1, LOW);
} else if ((angle_pitch_output > 1.01) && (angle_pitch_output < 2.00)) {
// Turn on Level LED
digitalWrite(levelLED_neg1, LOW);
digitalWrite(levelLED_neg0, LOW);
digitalWrite(levelLED_level, LOW);
digitalWrite(levelLED_pos0, HIGH);
digitalWrite(levelLED_pos1, LOW);
} else if (angle_pitch_output > 2.01) {
// Turn on Level LED
digitalWrite(levelLED_neg1, LOW);
digitalWrite(levelLED_neg0, LOW);
digitalWrite(levelLED_level, LOW);
digitalWrite(levelLED_pos0, LOW);
digitalWrite(levelLED_pos1, HIGH);
}
displaycount = 0;
}
while(micros() - loop_timer < 4000);
//Reset the loop timer
loop_timer = micros();
}
void setup_mpu_6050_registers(){
//Activate the MPU-6050
//Start communicating with the MPU-6050
Wire.beginTransmission(0x68);
//Send the requested starting register
Wire.write(0x6B);
//Set the requested starting register
Wire.write(0x00);
//End the transmission
Wire.endTransmission();
//Configure the accelerometer (+/-8g)
//Start communicating with the MPU-6050
Wire.beginTransmission(0x68);
//Send the requested starting register
Wire.write(0x1C);
//Set the requested starting register
Wire.write(0x10);
//End the transmission
Wire.endTransmission();
//Configure the gyro (500dps full scale)
//Start communicating with the MPU-6050
Wire.beginTransmission(0x68);
//Send the requested starting register
Wire.write(0x1B);
//Set the requested starting register
Wire.write(0x08);
//End the transmission
Wire.endTransmission();
}
void read_mpu_6050_data(){
//Read the raw gyro and accelerometer data
//Start communicating with the MPU-6050
Wire.beginTransmission(0x68);
//Send the requested starting register
Wire.write(0x3B);
//End the transmission
Wire.endTransmission();
//Request 14 bytes from the MPU-6050
Wire.requestFrom(0x68,14);
//Wait until all the bytes are received
while(Wire.available() < 14);
//Following statements left shift 8 bits, then bitwise OR.
//Turns two 8-bit values into one 16-bit value
acc_x = Wire.read()<<8|Wire.read();
acc_y = Wire.read()<<8|Wire.read();
acc_z = Wire.read()<<8|Wire.read();
temp = Wire.read()<<8|Wire.read();
gyro_x = Wire.read()<<8|Wire.read();
gyro_y = Wire.read()<<8|Wire.read();
gyro_z = Wire.read()<<8|Wire.read();
}
OK before we (I) look at the code, I don't believe your MPU 9250 sensor is accurate enough to warrant the 2nd decimal place and I'm not too sure about the 1st decimal place (00.x) either.
The nonlinearity for the acceleration portion of the device is +/-0.5% on the 2g version. I would think that would be 1% in you are only using 1/2 the range.
is there an IMU that may be accurate enough for this project? I'll dig around to see but this may be a limitation with most all of this sensor set up.
I think I'm starting to see why they use a form of rotary encoder to do this now.
You might find this post interesting.
This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.