-
Notifications
You must be signed in to change notification settings - Fork 0
/
MainV3.pde
39 lines (31 loc) · 943 Bytes
/
MainV3.pde
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
import processing.io.*;
PCA9685 pca9685;
MPU6050 mpu6050;
int servo_min = 200;
int servo_max = 650;
void setup() {
size(640, 480);
pca9685 = new PCA9685("i2c-1", 0x40);
pca9685.setServoPulse(15, 20);
pca9685.setServoPulse(14, 20);
pca9685.setPWMFreq(60);
mpu6050 = new MPU6050("i2c-1", 0x68);
}
void draw() {
background(0);
stroke(255);
//int temp = mpu6050.readTemp();
//float temp = mpu6050.readTemp();
float temp = mpu6050.readTemp();
float[] xyvals = mpu6050.readRotation();
float xval = xyvals[0];
println("Angulo Real: "+xval);
//println(round(temp));
float mapAngle = map(xval, -90, 90, servo_min, servo_max);
println("Angulo Mapeado: "+mapAngle);
pca9685.setPWM(15, 0, (int)mapAngle);
pca9685.setPWM(14, 0, servo_min);
line(width/2, height/2, width/2+sin(radians(xval))*width/2, height/2-cos(radians(xval))*height/2);
delay(50);
//pca9685.setPWM(15, 0, servo_max);
}