Drawing a line at a given angle

Question

Im attempting a device that measures lean angle. I have an ESP8266 + MPU6050 + SSD1306 setup, and I've managed to build a shaky but somewhat functioning device.

What Id like to do is use the Adafruit_SSD1306 library to call the display.drawLine(x, y, x1, y1, color) function and have the line drawn to the measured angle output of the MPU6050.

The screen I'm using is 128 X 64

The MPU6050 is initially laying flat at 180 degrees, but is set and displayed as 0 degrees of tilt. The line is drawn vertically on the screen and pivots around x1 and y1 at 64, 64 respectively in the middle at the bottom of the screen.

If the MPU6050 pivots left like in the picture, the angle measurement itself is roughly correct, but the line indicator is showing more like 30 degrees than 45, and gets worse the larger the angle value.

My code:

#include <Adafruit_MPU6050.h>
#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_Sensor.h>

const int MPU_addr=0x68;
int16_t axis_X,axis_Y,axis_Z;

Adafruit_MPU6050 mpu;
Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &Wire);

int minVal=265;
int maxVal=402;

double x;
double y;
double z;
int tiltAngle;
double y2;
 
void setup(){
  Serial.begin(9600);
  Serial.println("MPU6050 OLED demo");

  if (!mpu.begin()) {
    Serial.println("Sensor init failed");
    while (1)
      yield();
  }
  Serial.println("Found a MPU-6050 sensor");
  
    // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for (;;)
      ; // Don't proceed, loop forever
  }
  
  display.display();
  delay(500); // Pause for 2 seconds
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setRotation(0);
}
void loop(){
  display.clearDisplay();
  display.setCursor(0, 0);
  
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);
  axis_X=Wire.read()<<8|Wire.read();
  axis_Y=Wire.read()<<8|Wire.read();
  axis_Z=Wire.read()<<8|Wire.read();
    int xAng = map(axis_X,minVal,maxVal,-90,90);
    int yAng = map(axis_Y,minVal,maxVal,-90,90);
    int zAng = map(axis_Z,minVal,maxVal,-90,90);

     x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
     y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
     z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);

     tiltAngle = (int) x - 180; // we want the tilt angle to be 0 degrees
     display.println(" ");
     display.println(" ");
     display.println(" ");

     // display only positive angle values for either direction of tilt
     if(tiltAngle > 0){ display.print(tiltAngle);}
     else if (tiltAngle < 0) { display.print(tiltAngle * -1);}
     else {display.print(tiltAngle);}

     
     display.drawLine(-tiltAngle + 64, 0,64,64,WHITE);
     display.display();
     delay(100);  
}

Im aware that for my first y co-ordinate I will need to use some kind of formula to get the line to move down from 0 (the top of the screen) as the lean angle gets greater. I'm hoping someone could point me in the right direction for drawing the line at the angle given by the MPU6050.

I did consider using case statements for each angle value and drawing the line with fixed co-ordinates based on each angle value. But that seems too compute intensive and could take up too much of the DRAM. Is anyone aware of a simple formula I could use? Thanks for reading.

Answer 1

Use trigonometry (sin() and cos()) functions to get the right x and y coordinates.

I would consider replacing your drawLine() command with something like this (untested) assuming the tiltAngle is in degrees from -90 to +90 (with 0 degrees is straight up) and you want to draw a line at this angle from the origin (63, 63) with a radius of 64.

float tiltAngleRad = tiltAngle * DEG_TO_RAD; // convert angle to radians
int xEnd = 63 + 64 * sin (tiltAngleRad); // Ending x-coordinate offset & radius
int yEnd = 63 - 64 * cos (tiltAngleRad); // Ending y-coordinate offset & radius
display.drawLine (63, 63, xEnd, yEnd);

You might have to experiment with the offset and radius but I believe this will get you close yo what you want.