I reset via a button on the arduino
Here is the code:
#include <BMP180I2C.h>
#include <SD.h>
#include "Wire.h"
#include <ServoTimer2.h>
#include <VirtualWire.h>
#define BMP180_I2C_ADDRESS 0x77
#define MPU6050_I2C_ADDRESS 0x68
BMP180I2C bmp180(BMP180_I2C_ADDRESS);
File myFile;
ServoTimer2 S1; // create servo objects
ServoTimer2 S2;
ServoTimer2 S3;
float gyro[3];
float acc[3];
uint8_t buff[1] = {255};
uint8_t bufflen = sizeof(buff);
uint32_t pressure = 0;
#define l1_s1 14.5//14.5; //length of servo attach
#define l2_s1 26.26//30; //bar length
#define l3_s1 36.5//37; // distance from pin to bar on the 3d printed part
#define Cx_s1 27.91//31.608; //x distance between servo wheel and mount pin
#define Cy_s1 -20.665//-22.165; // y distance
float m_to_s1(float a){ //motor angle to servo angle function
a = a*PI/180;
//float l1_s1 = 14.5;//14.5; //length of servo attach
//float l2_s1 = 26.26;//30; //bar length
//float l3_s1 = 36.5;//37; // distance from pin to bar on the 3d printed part
//float Cx_s1 = 27.91;//31.608; //x distance between servo wheel and mount pin
//float Cy_s1 = -20.665;//-22.165; // y distance
float px = Cx_s1 + l3_s1*cos(a);
float py = Cy_s1 + l3_s1*sin(a);
float d = sqrt(pow(px,2)+pow(py,2));
float x1 = (pow(l1_s1,2) + pow(d,2) - pow(l2_s1,2)) / (2*d);
float y1 = sqrt(pow(l1_s1,2) - pow(x1,2));
float ang = atan2(py,px);
float x2 = x1*cos(ang)-y1*sin(ang);
float y2 = x1*sin(ang)+y1*cos(ang);
return (89 - 83.3102 + atan2(y2,x2)/PI*180); //before was 92
//return (93 - 83.62104236706482 + atan2(y2,x2)/PI*180); //offset: (93 experimental, 83.62.. calculated)
}
#define l1_s2 10.5//14.5;
#define l2_s2 25.12//33.5;
#define l3_s2 30.5//32.5;
#define Cx_s2 -26.759//-30.565;
#define Cy_s2 -31.335//-35.335;
float m_to_s2(float a){
a = a*PI/180;
//float l1_s2 = 10.5;//14.5;
//float l2_s2 = 25.12;//33.5;
//float l3_s2 = 30.5;//32.5;
//float Cx_s2 = -26.759;//-30.565;
//float Cy_s2 = -31.335;//-35.335;
float px = Cx_s2 + l3_s2*cos(a);
float py = Cy_s2 + l3_s2*sin(a);
float d = sqrt(pow(px,2)+pow(py,2));
float x1 = -(pow(l1_s2,2) + pow(d,2) - pow(l2_s2,2)) / (2*d);
float y1 = sqrt(pow(l1_s2,2) - pow(x1,2));
float ang = atan2(py,px);
float x2 = x1*cos(ang)+y1*sin(ang);
float y2 = x1*sin(ang)-y1*cos(ang);
return (43-71.3867+atan2(y2,x2)/PI*180); //old offset was 39 degrees, before switching from servo to servotimer2
//return (85 - 93.3514140156806 + atan2(y2,x2)/PI*180);
}
float pr_to_alt(uint32_t pr){
return (44330 * (1 - pow(pr/100800.0,1/5.255))); //put here today's pressure
}
float g_err0;
float g_err1;
float g_err2;
float a_err0;
float a_err1;
float a_err2;
uint8_t init_err = 0;
unsigned long tic=micros();
unsigned long toc;
unsigned long t0=millis();
unsigned long initial_time;
void get_gyro() {
Wire.beginTransmission(MPU6050_I2C_ADDRESS);
Wire.write(0x43); // Gyro data first register address 0x43
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_I2C_ADDRESS, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
gyro[0] = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
gyro[1] = (Wire.read() << 8 | Wire.read()) / 131.0;
gyro[2] = (Wire.read() << 8 | Wire.read()) / 131.0;
gyro[0] -= g_err0;//correct gyro
gyro[1] -= g_err1;
gyro[2] -= g_err2;
}
void get_acc() {
Wire.beginTransmission(MPU6050_I2C_ADDRESS);
Wire.write(0x3B); // Accellerometer data first register address 0x43
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_I2C_ADDRESS, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
acc[0] = (Wire.read() << 8 | Wire.read()) / 4096.0; // 4096 from datasheet
acc[1] = (Wire.read() << 8 | Wire.read()) / 4096.0;
acc[2] = (Wire.read() << 8 | Wire.read()) / 4096.0;
acc[0] -= a_err0;//correct accelerometer
acc[1] -= a_err1;
acc[2] -= a_err2;
}
void setup() {
S1.attach(8); //outer
S2.attach(9); //inner
pinMode(7,OUTPUT);//Relee
pinMode(13,OUTPUT);
S3.attach(10);//Chute
S1.write(map(m_to_s1(90),0,180,600,2400));
S2.write(map(m_to_s2(90),0,180,600,2400));
S3.write(map(25,0,180,600,2400));
//while(1);//used for servo calibration
if (!SD.begin(6)) {
init_err += 1;
}
vw_set_ptt_pin(A1);
vw_set_tx_pin(3);
vw_set_rx_pin(4);
vw_setup(2000);
vw_rx_start();
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
//Wire.setWireTimeout(3000, true); //timeout value in uSec
Serial.begin(9600);
while (!Serial);
Wire.beginTransmission(MPU6050_I2C_ADDRESS); // 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(MPU6050_I2C_ADDRESS);
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);
if (!bmp180.begin()) {
init_err += 2;
}
bmp180.resetToDefaults();
bmp180.setSamplingMode(BMP180MI::MODE_UHR);
digitalWrite(7,LOW);
while (buff[0]==255){ //waiting for signal
vw_get_message(buff, &bufflen);
}
vw_rx_stop();
//delay(500);
t0 = millis();
tic = micros();
bmp180.measureTemperature();
if (SD.exists(F("l.txt"))) SD.remove(F("l.txt"));
myFile = SD.open(F("l.txt"), FILE_WRITE);
//myFile.println(F("time\telaps\taction\ta1\ta2\tangle0\tangle1\tangle2\tgyro0\tgyro1\tgyro2\tacc0\tacc1\tacc2\taltitude"));
initial_time = millis();
//Serial.println("start");
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
float a1,a2;
float d1,d2;
float i1,i2;
unsigned long elaps;
float es[3];
float acc_angles[3];
float pe1,pe2,pe3;
float Kp = -1.5;
float Ki = -1;
float Kd = -0.15;
float alt = 0;
float prev_alt;
unsigned long chute_elaps; //checks how much time has passed after the last check_apogee()
uint8_t state = 0b00000010; //[bmp_var first bit, first_time_entering_action second bit]
//bmp_var tells whether to check temperature or pressure measurement
//first time entering action indicates if it's the first time entering a certain if{} statement or some other loop
uint8_t action;
int nloops = 0;
float acc_sum[3];
void loop() {
//Serial.print(action);
//Serial.print("\t");
Serial.println(millis()-t0);
toc=tic;
tic = micros();
elaps = tic-toc; //elapsed time in microseconds
get_gyro();//gets raw gyro data
get_acc();//gets raw acc
get_gyro_angles();
//get_acc_angles();
//Serial.println("");
bmp180_routine();
if (action==0) { //still
if (state & 2){ //if second bit is 1
buff[0] = init_err;
state ^= 0b10; //swaps second bit back to zero
vw_send((uint8_t *)buff, bufflen);
}
if (millis()-t0>5000) {
//Serial.println("debug");
pe1 = es[0];
pe2 = es[1];
pe3 = es[2];
prev_alt=alt; //initialize prev_alt
action=1;
buff[0] = 1; //buff[0] will contain action
state ^= 0b10;
t0=millis();
}
}
if (action==1) { //calibration of mpu, right before launch
nloops++;
acc_sum[0] += acc[0];
acc_sum[1] += acc[1];
acc_sum[2] += acc[2];
if (state & 2){ //if second bit is 1
state ^= 0b10; //swaps second bit back to zero
vw_send((uint8_t *)buff, bufflen);
}
if (millis()-t0>5000) {
int dh = millis()-t0;
g_err0 += (es[0]-pe1)/dh*1000;
g_err1 += (es[1]-pe2)/dh*1000;
g_err2 += (es[2]-pe3)/dh*1000;
a_err0 = acc_sum[0] / nloops;
a_err1 = acc_sum[1] / nloops - 1;
a_err2 = acc_sum[2] / nloops;
es[0]=0;
es[1]=0;
es[2]=0;
i1=0;
i2=0;
d1=0;
d2=0;
action=2;
buff[0] = 2;
state ^= 0b10;
t0=millis();
//myFile = SD.open(F("l.txt"), FILE_WRITE);
}
}
if (action==2) {//flight
if (state & 2){ //if second bit is 1
state ^= 0b10; //swaps second bit back to zero
digitalWrite(7,HIGH); //LAUNCH
chute_elaps = millis();
vw_send((uint8_t *)buff, bufflen);
}
PIDloop();
update_servo();
print_data();
/*if (millis()-t0>2000) {
if (millis() - chute_elaps > 500) {
chute_elaps=millis();
if (check_apogee()) {
S3.write(map(170,0,180,600,2400));
action = 3;
buff[0] = 3;
state ^= 0b10;
t0 = millis();
myFile.flush();
}
}
}*/
if (millis()-t0>10000) { //for test
action = 3;
buff[0] = 3;
state ^= 0b10;
t0 = millis();
myFile.flush();
}
}
if (action==3) {//parachute descent
if (state & 2){ //if second bit is 1
state ^= 0b10; //swaps second bit back to zero
vw_send((uint8_t *)buff, bufflen);
}
PIDloop();
update_servo();
print_data();
if (millis()-t0>25000) { //landing after x seconds
action=4;
buff[0] = 4;
state ^= 0b10;
t0=millis();
}
}
if (action==4) {//close file, still
if (state & 2){ //if second bit is 1
state ^= 0b10; //swaps second bit back to zero
vw_send((uint8_t *)buff, bufflen);
myFile.close();
digitalWrite(7,LOW);
}
}
}
bool check_apogee() {
if ((alt-prev_alt)<-1.2) {
return 1;
}
else {
prev_alt = alt;
return 0;
}
}
void bmp180_routine() {
if ( (state & 1) == 0) { //reads first bit of state
if (bmp180.hasValue()) {
bmp180.getTemperature();
bmp180.measurePressure();
state ^= 1; //uses XOR to switch the last bit of state to 1; old was bmp_var = 1;
}
}
else {
if (bmp180.hasValue()) {
pressure = bmp180.getPressure();
//alt = weight*alt + (1-weight)*pr_to_alt(pressure) ;//- zero_alt; //relative altitude to starting pos
alt = pr_to_alt(pressure) ;
bmp180.measureTemperature();
state ^= 1; //swaps last bit of state
}
}
}
void PIDloop() {
i1 += elaps*(es[0])/1000000;
i2 += elaps*(es[2])/1000000;
d1 = gyro[0];
d2 = gyro[2];
a1 = 90 + Kp*(es[0]) +Kd*d1 +Ki*i1;
a2 = 90 + Kp*(es[2]) +Kd*d2 +Ki*i2;
//bounds
if (a1>95) a1=95;
if (a1<85) a1=85;
if (a2>95) a2=95;
if (a2<85) a2=85;
if (Ki*i1>10) i1=10/Ki;
if (Ki*i1<-10) i1=-10/Ki;
if (Ki*i2>10) i2=10/Ki;
if (Ki*i2<-10) i2=-10/Ki;
}
void update_servo() {
//update servo position
S1.write(map(m_to_s1(a1),0,180,600,2400));
S2.write(map(m_to_s2(a2),0,180,600,2400));
}
void get_gyro_angles() {
es[0] += (elaps*(gyro[0]))/1000000;
es[1] += (elaps*(gyro[1]))/1000000;//roll
es[2] += (elaps*(gyro[2]))/1000000;
}
void get_acc_angles() {
acc_angles[0] = - RAD_TO_DEG * atan2(sqrt(pow(acc[1],2) + pow(acc[0],2)),acc[2]) + 90;
acc_angles[1] = 0;
acc_angles[2] = (RAD_TO_DEG * atan2(sqrt(pow(acc[1],2) + pow(acc[2],2)),acc[0]) -90 );
}
void print_data() {
myFile.print(millis()-initial_time);
myFile.print("\t");
myFile.print(elaps);
myFile.print("\t");
myFile.print(action);
myFile.print("\t");
myFile.print(a1);
myFile.print("\t");
myFile.print(a2);
myFile.print("\t");
myFile.print(es[0]);
myFile.print("\t");
myFile.print(es[1]);
myFile.print("\t");
myFile.print(es[2]);
myFile.print("\t");
myFile.print(gyro[0]);
myFile.print("\t");
myFile.print(gyro[1]);
myFile.print("\t");
myFile.print(gyro[2]);
myFile.print("\t");
myFile.print(acc[0]);
myFile.print("\t");
myFile.print(acc[1]);
myFile.print("\t");
myFile.print(acc[2]);
myFile.print("\t");
myFile.println(alt,2);
}
And here's the code of the other arduino, that sends the first signal:
//#include <RH_ASK.h>
//#include <SPI.h>
#include <VirtualWire.h>
//RH_ASK mod(2000, 7, 6 , A1);
uint8_t buff[1]={0};
uint8_t bufflen = 1;
long int t0;
#define r 3
#define g 5
#define b 11
void setup() {
vw_set_ptt_pin(A1);
vw_set_tx_pin(6);
vw_set_rx_pin(7);
vw_setup(2000);
pinMode(r,OUTPUT);
pinMode(g,OUTPUT);
pinMode(b,OUTPUT);
analogWrite(r,255);
analogWrite(g,0);
analogWrite(b,0);
// put your setup code here, to run once:
Serial.begin(115200);
pinMode(13,OUTPUT);
//if (!mod.init()){
// Serial.println("module init failed");
//}
//if (!ric.init()){
// Serial.println("receiver init failed");
//}
vw_rx_start();
Serial.println("\nmodule init success");
//while(Serial.available() == 0) {}
Serial.println("packet transmitted");
delay(100);
vw_send((uint8_t *)buff, bufflen);
//vw_wait_tx();
t0 = millis();
}
void loop() {
//digitalWrite(13, buff[0]);
// put your main code here, to run repeatedly:
//mod.send((uint8_t *)buff, bufflen);
//mod.send((uint8_t *)start, 1);
//mod.waitPacketSent();
//buff[0] = !buff[0];
/*if (!mod.recv(buff, &bufflen)){
Serial.println("pacchetto non ricevuto");
}
else{
digitalWrite(5, buff[0]);
}*/
//if (mod.recv(buff, &bufflen)) {
if (vw_get_message(buff, &bufflen)) {
Serial.print((millis()-t0)/1000.0,3);
Serial.print("\tsec\t");
switch (buff[0]) {
case (0) :
analogWrite(r,0);
analogWrite(g,255);
analogWrite(b,0);
Serial.println("ack");
break;
case (1) :
analogWrite(r,0);
analogWrite(g,255);
analogWrite(b,255);
Serial.println("calibration");
break;
case (2) :
analogWrite(r,0);
analogWrite(g,0);
analogWrite(b,255);
Serial.println("launch");
break;
case (3) :
analogWrite(r,255);
analogWrite(g,255);
analogWrite(b,255);
Serial.println("chute");
break;
/*case (10) :
analogWrite(r,0);
analogWrite(g,0);
analogWrite(b,0);
break;
case (11) :
analogWrite(r,0);
analogWrite(g,0);
analogWrite(b,0);
break;
case (12) :
analogWrite(r,0);
analogWrite(g,0);
analogWrite(b,0);
break;
case (13) :
analogWrite(r,0);
analogWrite(g,0);
analogWrite(b,0);
break;*/
}
}
//delay(500);
}