void loop()
{
MPU6050::Values values;
float norme;
mpu.readValues(&values, NULL);
logger.loopWithValues(values.xAccel, values.yAccel, values.zAccel, values.xGyro, values.yGyro, values.zGyro);
norme = sqrtf((float)values.xAccel * (float)values.xAccel + (float)values.yAccel * (float)values.yAccel + (float)values.zAccel * (float)values.zAccel) / 32767.0 * 16.0;
if (norme < accelLimit && !ballTurnedOn) {
ballTurnedOn = true;
setBallLight(0xFFFFFF);
} else if (norme > 0.6 && ballTurnedOn) {
ballTurnedOn = false;
setBallLight(0x000000);
}
}
void ball(void)
{
static double speed = 5;
static double friction = 0.01;
static double position = 0;
double pi = 3.14159;
MPU6050::Values values;
double forceAngle = 00;
double forceNorme = 0;
uint8_t ii, count, led;
Serial.print("position ");
Serial.println(position);
mpu.readValues(&values, NULL);
Serial.print("x accel ");
Serial.print(values.xAccel);
Serial.print(" y accel ");
Serial.println(values.yAccel);
values.yAccel = -values.yAccel;
if (values.xAccel == 0) {
if (values.yAccel < 0) {
forceAngle = -90;
} else {
forceAngle = 90;
}
} else {
forceAngle = atan((double)values.yAccel / (double)values.xAccel) * 180 / pi;
if (values.xAccel < 0) {
forceAngle -= 180;
}
}
forceNorme = sqrt(values.yAccel * values.yAccel + values.xAccel * values.xAccel);
forceNorme = forceNorme / 50000.0;
Serial.print("angle ");
Serial.print(forceAngle);
Serial.print(" diff angle ");
Serial.print(position - forceAngle);
Serial.print(" force ");
Serial.println(sin((position - forceAngle) * pi / 180.0) * forceNorme);
speed -= sin((position - forceAngle) * pi / 180.0) * forceNorme;
if (speed > 0 && speed > friction) {
speed -= friction;
} else if (speed < 0 && -speed > friction) {
speed += friction;
} else {
speed = 0;
}
position += speed;
while (position > 360) {
position -= 360;
}
while (position < 0) {
position += 360;
}
count = strip1.numPixels();
led = position * count / 360.0;
for (ii = 0; ii < count; ii++) {
if (led == ii) {
strip1.setPixelColor(ii, 0x0f0f0f);
} else {
strip1.setPixelColor(ii, 0);
}
}
Serial.print("position ");
Serial.print(position);
Serial.print(" speed ");
Serial.print(speed);
Serial.print(" led ");
Serial.println(led);
strip1.show();
}
