Update Servo Control Mode

pull/1/head
Mathis et Yohan 2021-12-27 22:57:31 +01:00
rodzic cf50b015d4
commit cb53424159
2 zmienionych plików z 60 dodań i 35 usunięć

Plik binarny nie jest wyświetlany.

Wyświetl plik

@ -4,7 +4,7 @@
#include <SBUS.h> #include <SBUS.h>
#include <EasyBuzzer.h> #include <EasyBuzzer.h>
#include <Watchdog.h> #include <Watchdog.h>
#include <Servo.h> #include <PWMServo.h>
#include <movingAvg.h> #include <movingAvg.h>
#include <Adafruit_NeoPixel.h> #include <Adafruit_NeoPixel.h>
#include <EEPROM.h> #include <EEPROM.h>
@ -13,17 +13,17 @@
#define i_want_to_fly false // Simulated servo movement to test the servo movement :)) #define i_want_to_fly false // Simulated servo movement to test the servo movement :))
#define buzzer_turn false // Buzzer sounds as function of the turn command #define buzzer_turn false // Buzzer sounds as function of the turn command
#define no_init true // Skip init, for testing only purposes #define no_init false // Skip init, for testing only purposes
#define rc_mode 2 // 0 = only roll, 1 = pitch and roll mixed, 2 = pitch and roll separated #define rc_mode 1 // 0 = only roll, 1 = pitch and roll mixed, 2 = pitch and roll separated
#define autopilot_mode 1 // Control linear or with "hands up" #define autopilot_mode 1 // Control linear or with "hands up"
#define drop false // R2Home's version, drop or motorised #define drop true // R2Home's version, drop or motorised
#define record_home false // only record autopilot #define record_home false // only record autopilot
#define dep_alt 100 // m above ground #define dep_alt 100 // m above ground
#define vup 1.5 // m/s #define vup 1.5 // m/s
#define vdown 2 // m/s #define vdown 2 // m/s
#define gps_freq 5 // Hz #define gps_freq 5 // Hz
#define nav_waypoint true // Doing the waypoint sequence before reaching home? #define nav_waypoint true // Doing the waypoint sequence before reaching home?
#define nav_home false // Should it go home after the waypoint sequence? #define nav_home true // Should it go home after the waypoint sequence?
#define sd_writing true // Should it write on the SD card or not? #define sd_writing true // Should it write on the SD card or not?
#define time_out 300 #define time_out 300
@ -167,11 +167,11 @@ int count = 0;
// SERVO / // SERVO /
movingAvg st(5); movingAvg st(5);
Servo steer; PWMServo steer;
Servo deploy; PWMServo deploy;
Servo left; PWMServo left;
Servo right; PWMServo right;
Servo esc; PWMServo esc;
float roll_man = 1500; float roll_man = 1500;
float pitch_man = 1500; float pitch_man = 1500;
@ -292,7 +292,7 @@ void setup() {
strip.begin(); strip.begin();
strip.show(); strip.show();
strip.setBrightness(BRIGHTNESS); strip.setBrightness(255);
tone(buzzer, 523); tone(buzzer, 523);
delay(200); delay(200);
@ -605,7 +605,6 @@ void datacmpt() {
String g_text = String(channels[6]); String g_text = String(channels[6]);
String rc_text = a_text+","+b_text+","+c_text+","+d_text+","+e_text+","+f_text+","+g_text; String rc_text = a_text+","+b_text+","+c_text+","+d_text+","+e_text+","+f_text+","+g_text;
String servo_text; String servo_text;
if (drop == true) { if (drop == true) {
@ -842,7 +841,7 @@ void flight_init() {
dep_altitude = (dep_altitude+gps.altitude.meters()); dep_altitude = (dep_altitude+gps.altitude.meters());
strip.setBrightness(50); strip.setBrightness(255);
setcam(1); setcam(1);
if (record_home == false) { newfile(); } if (record_home == false) { newfile(); }
@ -895,22 +894,22 @@ void ready_steady() {
if (millis()-init_time>=1000) { if (millis()-init_time>=1000) {
if (vspeed>vup) { flight_mode = 2; EasyBuzzer.beep(1000,100,50,2,500,1); strip.setBrightness(75); } if (vspeed>vup) { flight_mode = 2; EasyBuzzer.beep(1000,100,50,2,500,1); strip.setBrightness(255); }
if (vspeed<vdown) { flight_mode = 3; EasyBuzzer.beep(3000,100,50,3,500,1); strip.setBrightness(100); } if (vspeed<vdown) { flight_mode = 3; EasyBuzzer.beep(3000,100,50,3,500,1); strip.setBrightness(255); }
if ((atof(fix_type.value()) < 2) and (no_init == false)) { flight_mode = 0; EasyBuzzer.beep(700,100,50,3,500,1); strip.setBrightness(50); } if ((atof(fix_type.value()) < 2) and (no_init == false)) { flight_mode = 0; EasyBuzzer.beep(700,100,50,3,500,1); strip.setBrightness(255); }
} }
} }
//------------------- 2 -------------------// //------------------- 2 -------------------//
void flight_ascent() { if (vspeed<0.5) {flight_mode = 1; strip.setBrightness(50);} } void flight_ascent() { if (vspeed<0.5) {flight_mode = 1; strip.setBrightness(255);} }
//------------------- 3 -------------------// //------------------- 3 -------------------//
void flight_descent() { void flight_descent() {
if (vspeed>-0.5) {flight_mode = 1; strip.setBrightness(50);} if (vspeed>-0.5) {flight_mode = 1; strip.setBrightness(255);}
if (merged_alt < dep_altitude) { flight_mode = 4; EasyBuzzer.beep(3000,100,50,5,500,1); strip.setBrightness(255); deployed = true; init_time = millis(); } if (merged_alt < dep_altitude) { flight_mode = 4; EasyBuzzer.beep(3000,100,50,5,500,1); strip.setBrightness(255); deployed = true; init_time = millis(); }
} }
@ -922,8 +921,8 @@ void flight_gliding() {
if (((millis()-init_time) >= gliding_timer) or (vspeed > (vdown+1))) { if (((millis()-init_time) >= gliding_timer) or (vspeed > (vdown+1))) {
wing_opened = true; wing_opened = true;
last_errHome = errHome; last_errHome = errHome;
if (failSafe == true) {flight_mode = 5; strip.setBrightness(100);} if (failSafe == true) {flight_mode = 5; strip.setBrightness(255);}
else {flight_mode = 6; strip.setBrightness(100);} else {flight_mode = 6; strip.setBrightness(255);}
} }
} }
@ -948,7 +947,7 @@ void flight_gliding_manual() {
//------------------- 7 -------------------// //------------------- 7 -------------------//
void landed() { if ((baro_stab == false) or (gps_stab == false)) {flight_mode = 1; strip.setBrightness(50);} } void landed() { if ((baro_stab == false) or (gps_stab == false)) {flight_mode = 1; strip.setBrightness(255);} }
//------------------- 8 -------------------// //------------------- 8 -------------------//
@ -995,14 +994,27 @@ void applycmd() {
break; break;
case 1: case 1:
servo_left = roll_man; servo_left = roll_man-500;
servo_right = 3000-roll_man; servo_right = 2500-roll_man;
servo_left = servo_left+(pitch_man-1500); servo_left = servo_left+(pitch_man-1500);
servo_right = servo_right+(pitch_man-1500); servo_right = servo_right+(pitch_man-1500);
servo_left = constrain(servo_left, servo_man_min, servo_man_max); servo_left = constrain(servo_left, servo_man_min, servo_man_max);
servo_right = constrain(servo_left, servo_man_min, servo_man_max); if (servo_left>(servo_man_min+10)) {
servo_left = map(servo_left, 1000, 2000, servo_left_start, 2000);
}
else {
servo_left = servo_left_max;
}
servo_right = constrain(servo_right, servo_man_min, servo_man_max);
if (servo_right>(servo_man_min+10)) {
servo_right = map(servo_right, 1000, 2000, servo_right_start, 2000);
}
else {
servo_right = servo_right_max;
}
break; break;
case 2: case 2:
@ -1028,8 +1040,21 @@ void applycmd() {
servo_left = servo_left+(pitch_man-1500); servo_left = servo_left+(pitch_man-1500);
servo_right = servo_right+(pitch_man-1500); servo_right = servo_right+(pitch_man-1500);
servo_left = constrain(servo_left, servo_man_min, servo_man_max); servo_left = constrain(servo_left, servo_man_min, servo_man_max);
servo_right = constrain(servo_left, servo_man_min, servo_man_max); if (servo_left>(servo_man_min+10)) {
servo_left = map(servo_left, 1000, 2000, servo_left_start, 2000);
}
else {
servo_left = servo_left_max;
}
servo_right = constrain(servo_right, servo_man_min, servo_man_max);
if (servo_right>(servo_man_min+10)) {
servo_right = map(servo_right, 1000, 2000, servo_right_start, 2000);
}
else {
servo_right = servo_right_max;
}
break; break;
case 2: case 2:
@ -1093,7 +1118,7 @@ void applycmd() {
else { else {
if (failSafe == true) { servo_deploy = 2000; } if (failSafe == true) { servo_deploy = 2000; }
else { servo_deploy = map(channels[6], 50, 1950, 1000, 2000);} else { servo_deploy = map(channels[6], 50, 1950, 1000, 2000);}
} }
break; break;
case 8: case 8:
@ -1124,9 +1149,9 @@ if (drop == true) {
if ((millis()-tpwm)>=(1000/a)) { if ((millis()-tpwm)>=(1000/a)) {
tpwm = millis(); tpwm = millis();
left.writeMicroseconds(servo_left); left.write(map(servo_left, 1000, 2000, 0, 180));
right.writeMicroseconds(servo_right); right.write(map(servo_right, 1000, 2000, 0, 180));
deploy.writeMicroseconds(servo_deploy); deploy.write(map(servo_deploy, 1000, 2000, 0, 180));
} }
} }
@ -1136,9 +1161,9 @@ else {
if ((millis()-tpwm)>=(1000/a)) { if ((millis()-tpwm)>=(1000/a)) {
tpwm = millis(); tpwm = millis();
left.writeMicroseconds(servo_left); left.write(map(servo_left, 1000, 2000, 0, 180));
right.writeMicroseconds(servo_right); right.write(map(servo_right, 1000, 2000, 0, 180));
esc.writeMicroseconds(servo_esc); esc.write(map(servo_esc, 1000, 2000, 0, 180));
} }
} }
@ -1208,7 +1233,7 @@ void setled(int a, int b, int c, int d, int e) {
if ((millis()-lastled)>=e) { if ((millis()-lastled)>=e) {
lastled = millis(); lastled = millis();
colorWipe(strip.Color(b,a,c), 0); colorWipe(strip.Color(a,b,c), 0);
duration = d; duration = d;
timeled = millis(); timeled = millis();
} }