#include <Wire.h>
unsigned int time = 0;
const int MPU = 0x68; // MPU6050 I2C address
float AccX, AccY, AccZ;
void setup() {
Serial.begin(19200);
Wire.begin(); // Initialize comunication
Wire.beginTransmission(MPU); // Start communication with MPU6050 // MPU=0x68
Wire.write(0x6B); // Talk to the register 6B
Wire.write(0x00); // Make reset - place a 0 into the 6B register
Wire.endTransmission(true); //end the transmission
/*
// Configure Accelerometer Sensitivity - Full Scale Range (default +/- 2g)
Wire.beginTransmission(MPU);
Wire.write(0x1C); //Talk to the ACCEL_CONFIG register (1C hex)
Wire.write(0x10); //Set the register bits as 00010000 (+/- 8g full scale range)
Wire.endTransmission(true);
// Configure Gyro Sensitivity - Full Scale Range (default +/- 250deg/s)
Wire.beginTransmission(MPU);
Wire.write(0x1B); // Talk to the GYRO_CONFIG register (1B hex)
Wire.write(0x10); // Set the register bits as 00010000 (1000deg/s full scale)
Wire.endTransmission(true);
delay(20);
*/
// Call this function if you need to get the IMU error values for your module
delay(20);
pinMode(A0, INPUT); // Set the A0 pin to a input mode
pinMode(A1, INPUT); // Set the A1 pin to a input mode
pinMode(A2, INPUT); // Set the A2 pin to a input mode
pinMode(A3, INPUT); // Set the A3 pin to a input mode
pinMode(2, INPUT_PULLUP); // Set the 3 pin to a input mode
}
int sport = 0;
int changeS = 0;
int raise = 0;
int lower = 0;
int changeUP;
int changeDWN;
int level;
// values below in g
float brakepoint1 = -0.2; // first threshold during braking
float brakepoint2 = -0.4; // second threshold during braking
float brakepoint3 = -0.6; // third threshold during braking
float accelpoint1 = 0.2; // first threshold during acceleration
float accelpoint2 = 0.4; // second threshold during acceleration
float accelpoint3 = 0.6; // third threshold during acceleration
float accelcorneringpoint1 = 0.2; // first threshold acceleration in cornering
float accelcorneringpoint2 = 0.4; // second threshold acceleration in cornering
float accelcorneringpoint3 = 0.6; // third threshold acceleration in cornering
float sinkingpoint1 = 0.1; // first threshold acceleration for sinking
float sinkingpoint2 = 0.2; // second threshold acceleration in sinking
float sinkingpoint3 = 0.3; // third threshold acceleration in sinking
void loop() {
// time = millis();
// === Read accelerometer data === //
Wire.beginTransmission(MPU);
Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers
//For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value
AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value
//delay(500);
changeS = sport;
if (digitalRead(A2) == HIGH) sport = 1;
if (digitalRead(A1) == HIGH) sport = 0;
if (changeS != sport) {
digitalWrite(12, HIGH);
digitalWrite(11, HIGH);
digitalWrite(10, HIGH);
digitalWrite(9, HIGH);
digitalWrite(8, HIGH);
digitalWrite(7, HIGH);
}
// Print out the values
/*
Serial.print("X = ");
Serial.print(AccX);
Serial.print(", Y = ");
Serial.print(AccY);
Serial.print(", Z = ");
Serial.println(AccZ);
*/
switch (sport) {
case 1:
digitalWrite(12, LOW);
digitalWrite(11, LOW);
digitalWrite(10, LOW);
digitalWrite(9, LOW);
digitalWrite(8, LOW);
digitalWrite(7, LOW);
break;
case 0:
// acceleration values in g
// + Y acc is acceleration ; - Y acc is braking
//Locking rear dampers during acceleration
if (AccY >= 0.05) {
if (AccY >= 0 && AccY < accelpoint1) {
digitalWrite(8, HIGH);
digitalWrite(7, HIGH);
}
if (AccY >= accelpoint1 && AccY < accelpoint2) {
digitalWrite(8, LOW);
digitalWrite(7, HIGH);
}
if (AccY >= accelpoint2 && AccY < accelpoint3) {
digitalWrite(7, LOW);
digitalWrite(8, HIGH);
}
if (AccY >= accelpoint3) digitalWrite(8, LOW);
}
// Locking front dampers during braking
if (AccY <= -0.05) {
if (AccY <= 0 && AccY > brakepoint1) {
digitalWrite(12, HIGH);
digitalWrite(11, HIGH);
digitalWrite(10, HIGH);
digitalWrite(9, HIGH);
}
if (AccY <= brakepoint1 && AccY > brakepoint2) {
digitalWrite(12, LOW);
digitalWrite(11, HIGH);
digitalWrite(10, LOW);
digitalWrite(9, HIGH);
}
if (AccY <= brakepoint2 && AccY > brakepoint3) {
digitalWrite(11, LOW);
digitalWrite(12, HIGH);
digitalWrite(9, LOW);
digitalWrite(10, HIGH);
}
if (AccY <= brakepoint3) {
digitalWrite(12, LOW);
digitalWrite(11, LOW);
digitalWrite(10, LOW);
digitalWrite(9, LOW);
}
}
// + Z acc is to the left ; - Z acc is to the right
// Locking left front damper during cornering to right
if (AccZ <= -0.05) {
if (AccZ <= 0 && AccZ > -1 * (accelcorneringpoint1)) {
digitalWrite(12, HIGH);
digitalWrite(11, HIGH);
}
if (AccZ <= -1 * (accelcorneringpoint1) && AccZ > -1 * (accelcorneringpoint2)) {
digitalWrite(12, LOW);
digitalWrite(11, HIGH);
}
if (AccZ <= -1 * (accelcorneringpoint2) && AccZ > -1 * (accelcorneringpoint3)) {
digitalWrite(11, LOW);
digitalWrite(12, HIGH);
}
if (AccZ <= -1 * (accelcorneringpoint3)) {
digitalWrite(12, LOW);
digitalWrite(11, LOW);
}
}
// Locking right front damper during cornering to left
if (AccZ >= 0.05) {
if (AccZ >= 0 && AccZ < accelcorneringpoint1) {
digitalWrite(10, HIGH);
digitalWrite(9, HIGH);
}
if (AccZ >= accelcorneringpoint1 && AccZ < accelcorneringpoint2) {
digitalWrite(10, LOW);
digitalWrite(9, HIGH);
}
if (AccZ >= accelcorneringpoint2 && AccZ < accelcorneringpoint3) {
digitalWrite(9, LOW);
digitalWrite(10, HIGH);
}
if (AccZ >= accelcorneringpoint3) {
digitalWrite(9, LOW);
digitalWrite(10, LOW);
}
}
// + X acc is up ; - X acc is down
// Locking dampers during sinking
if (AccX <= 0.99 && AccX > 1-sinkingpoint1) {
digitalWrite(12, HIGH);
digitalWrite(11, HIGH);
digitalWrite(10, HIGH);
digitalWrite(9, HIGH);
digitalWrite(8, HIGH);
digitalWrite(7, HIGH);
}
if (AccX <= 1-sinkingpoint1 && AccX > 1-sinkingpoint2) {
digitalWrite(12, LOW);
digitalWrite(11, HIGH);
digitalWrite(10, LOW);
digitalWrite(9, HIGH);
digitalWrite(8, LOW);
digitalWrite(7, HIGH);
}
if (AccX <= 1-sinkingpoint2 && AccX > 1-sinkingpoint3) {
digitalWrite(11, LOW);
digitalWrite(12, HIGH);
digitalWrite(9, LOW);
digitalWrite(10, HIGH);
digitalWrite(8, HIGH);
digitalWrite(7, LOW);
}
if (AccX <= 1-sinkingpoint3) {
digitalWrite(12, LOW);
digitalWrite(11, LOW);
digitalWrite(10, LOW);
digitalWrite(9, LOW);
digitalWrite(8, LOW);
digitalWrite(7, LOW);
}
break;
}
// raising & lowering vehicle for linear electric actuators - how long is pressed that long is raising or lowering
raise = digitalRead(A3);
if (raise == HIGH) {
digitalWrite(6, HIGH);
digitalWrite(5, HIGH);
}
else if (raise == LOW) {
digitalWrite(6, LOW);
digitalWrite(5, LOW);
}
lower = digitalRead(A0);
if (lower == HIGH) {
digitalWrite(6, HIGH);
digitalWrite(4, HIGH);
}
else if (lower == LOW) {
digitalWrite(6, LOW);
digitalWrite(4, LOW);
}
// MIL lit if oil level drops
int level = digitalRead(2);
if (level == 1) {
digitalWrite(3, HIGH);
}
else {
digitalWrite(3, LOW);
}
// time = millis() - time;
//Serial.println(time, DEC);
// delay(1000);
}