Created
July 18, 2019 09:06
-
-
Save brian-kiplagat/1a0b492f1bbb10ef4eb4e9ce5710caf2 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/*This arduino controls orientation...it is always on standby and is only activated by the master arduino | |
immedietly the rocket fires.the function of this is to make sure the | |
rocket is always on the right way up for takeoff. | |
THIS SYSTEM IS ON THE I2C BUS HENCE IS A SLAVE | |
* *Has a unique adress to identify it from all other devices on the bus | |
however this system is not a must since its function is too regulate it to | |
vertical flight...this is only for best precision... | |
more attention is to be given to the landing system....... | |
*/ | |
#include <Wire.h> | |
#include <Servo.h>//include servo library | |
Servo a1, b1, c1, d1; //define four servo objects | |
//-----SETUP ACCELEROMETER--------------- | |
const int MPU_addr = 0x68; | |
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ; | |
int minVal = 265; | |
int maxVal = 402; | |
double x; | |
double y; | |
double z; | |
//------------------------------------------------ | |
void setup() { | |
//---------------------------------------------------------- | |
// initialize the serial port: | |
Serial.begin(9600); | |
//-----------SETUP ACCELEROMETER-------------- | |
Wire.begin(); | |
Wire.beginTransmission(MPU_addr); | |
Wire.write(0x6B); | |
Wire.write(0); | |
Wire.endTransmission(true); | |
//------------SETUP SERVOS--------------------- | |
a1.attach(3);//attach servos to pwm pins | |
b1.attach(5); | |
c1.attach(6); | |
d1.attach(9); | |
a1.write(90);//take servo position to the middle(90) | |
b1.write(90); | |
c1.write(90); | |
d1.write(90); | |
} | |
void loop() { | |
//------------DATA from mpu---------------- | |
Wire.beginTransmission(MPU_addr); | |
Wire.write(0x3B); | |
Wire.endTransmission(false); | |
Wire.requestFrom(MPU_addr, 14, true); | |
AcX = Wire.read() << 8 | Wire.read(); | |
AcY = Wire.read() << 8 | Wire.read(); | |
AcZ = Wire.read() << 8 | Wire.read(); | |
int xAng = map(AcX, minVal, maxVal, -90, 90); | |
int yAng = map(AcY, minVal, maxVal, -90, 90); | |
int zAng = map(AcZ, 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); | |
/* Serial.print("AngleX= "); | |
Serial.println(x); | |
Serial.print("AngleY= "); | |
Serial.println(y); | |
Serial.print("AngleZ= "); | |
Serial.println(z); | |
Serial.println("-----------------------------------------"); | |
*/ | |
int xpos1 = map(x, 0, -90, 0, 90); //directly map change in x to servo b1 and d1 | |
int xpos2 = map(x, 0, 90, 90, 180); //directly map change in x to servo b1 and d1 | |
int ypos1 = map(y, 0, -90, 0, 90); //directly map change in x to servo b1 and d1 | |
int ypos2 = map(y, 0, 90, 90, 180); //directly map change in x to servo b1 and d1 | |
c1.write(xpos2);//slighly reduce surface area to correct orient | |
d1.write(xpos1);//slighly reduce surface area to correct orient | |
b1.write(ypos2);//slighly reduce surface area to correct orient | |
a1.write(ypos1);//slighly reduce surface area to correct orient | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment