update the servo control code

pull/1/head
Mathis et Yohan 2022-02-09 13:55:52 +01:00
rodzic b11c71d2f9
commit 0fdd245316
4 zmienionych plików z 99 dodań i 114 usunięć

Plik binarny nie jest wyświetlany.

Plik binarny nie jest wyświetlany.

Plik binarny nie jest wyświetlany.

Wyświetl plik

@ -12,21 +12,24 @@
// ----------------------------------- SETUP PANEL ----------------------------------- //
#define i_want_to_fly false // Simulated servo movement to test the servo movement :))
#define test_dir_rc false // Use channels 0 on the radio to test the direction of the autopilot and the servos, i_want_to_fly should be set true too.
#define buzzer_turn false // Buzzer sounds as function of the turn command
#define buzzer_sweep false // Buzzer turn on steroïds, should be easier to understand his tricky language ^^
#define no_init false // Skip init, for testing only purposes
#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 drop true // R2Home's version, drop or motorised
#define rc_mode 1 // only roll (0), pitch and roll mixed (1), pitch and roll separated (2)
#define control_mode 1 // neutral position at the center (0) or with "hands up" (1)
#define linear_mode 0 // command is linear (0), or linear but with a large deadband set using servo_start etc (1)
#define drop false // R2Home's version, drop or motorised
#define record_home false // only record autopilot
#define dep_alt 20000 // m above ground
#define vup 2 // m/s
#define vdown -2 // m/s
#define gps_freq 5 // Hz
#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_waypoint false // Doing the waypoint sequence before reaching home?
#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 low_rate false // Dataloging at 1HZ instead of 20Hz, for balloon flight
#define low_rate false // Dataloging at low HZ (if true) instead of 20Hz, for balloon flight
#define led_model 0 // Big led = 1, small led = 0.
#define time_out 300
@ -50,6 +53,10 @@
#define servo_right_start 1300
#define servo_right_max 1000
#define trig 10
#define left_offset 100
#define right_offset 100
#define gliding_timer 2500
#define waypoint_threshold 10 // Distance to waypoint before going to the next waypoint
@ -159,6 +166,7 @@ struct gps_location {
};
gps_location waypoint[17];
int waypoint_number = 0;
int last_waypoint_number = 0;
@ -298,6 +306,15 @@ void setup() {
if (drop == true) { deploy.attach(8, 1000, 2000); }
else { esc.attach(8, 1000, 2000); }
tone(buzzer, 523);
delay(200);
tone(buzzer, 582);
delay(200);
tone(buzzer, 762);
delay(200);
noTone(buzzer);
watchdog.enable(Watchdog::TIMEOUT_1S);
getconfig();
bmp.begin(baro_adress);
@ -311,14 +328,6 @@ void setup() {
strip.show();
strip.setBrightness(255);
tone(buzzer, 523);
delay(200);
tone(buzzer, 582);
delay(200);
tone(buzzer, 762);
delay(300);
noTone(buzzer);
EEPROM.get(0, reboot_state);
if (reboot_state == 1) {
@ -333,8 +342,11 @@ void setup() {
flight_rebooted = true;
}
watchdog.enable(Watchdog::TIMEOUT_1S);
tone(buzzer, 582);
delay(200);
tone(buzzer, 830);
delay(300);
noTone(buzzer);
}
void loop() {
@ -445,24 +457,30 @@ void datacmpt() {
if (new_cog == true) {
raterror = getangle((last_errHome+180),(errHome+180))*gps_freq ; raterror = rs.reading(raterror);
ratecog = getangle(prev_cog_b,gps.course.deg())*gps_freq; ratecog = rc.reading(ratecog);
raterror = getangle(last_errHome+180,errHome+180)*gps_freq;
//raterror = rs.reading(raterror);
float PIDsum = ((NKp*errHome)+(NKd*raterror));
ratecog = getangle(prev_cog_b,gps.course.deg())*gps_freq;
//ratecog = rc.reading(ratecog);
float PIDsum = (NKp*errHome)+(NKd*raterror);
//cmd_mult = (0.09999996 + (1 - 0.09999996)/(1 + pow(((abs(ratecog))/49.54231),24.26521))); cmd_mult = mult.reading(cmd_mult*1000); cmd_mult = (cmd_mult/1000);
cmd_mult = 1;
cmdHome = PIDsum*cmd_mult ;
if (vspeed<-5) { spiral = true; }
if (vspeed>-3) { spiral = false; }
if (vspeed<vdown-5) { spiral = true; }
if (vspeed>vdown-3) { spiral = false; }
if (spiral == true) { cmdHome = 0; }
new_cog = false;
last_errHome = errHome;
prev_cog_b = gps.course.deg();
float cmdRate = cmdHome-ratecog;
steer_auto = map(cmdHome, -180, +180, 1000, 2000);
steer_auto = constrain(steer_auto, servo_auto_min, servo_auto_max);
@ -653,7 +671,7 @@ void datacmpt() {
case 6:
case 8:
case 9:
if (low_rate) { delaySD = 1000; }
if (low_rate) { delaySD = 200; }
else { delaySD = 50; }
delayTLM = 1000;
break;
@ -981,116 +999,78 @@ void applycmd() {
case 0:
case 1:
case 6:
case 8:
if (failSafe == false) {
// ---------- Stage 1 - RC mode ---------- //
switch (rc_mode) {
case 0:
case 0: // roll only
servo_right = roll_man;
servo_left = 3000-roll_man;
break;
case 1:
servo_right = roll_man-500;
servo_left = 2500-roll_man;
case 1: // pitch and roll mixed
servo_right = roll_man;
servo_left = 3000-roll_man;
servo_left = servo_left+(pitch_man-1500);
servo_right = servo_right+(pitch_man-1500);
servo_left = 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;
case 2:
case 2: // pitch and roll separated
servo_left = roll_man;
servo_right = pitch_man;
break;
}
}
else { servo_left = 1000; servo_right = 1000; }
break;
case 8:
switch (rc_mode) {
case 0:
servo_right = roll_man;
servo_left = 3000-roll_man;
break;
case 1:
servo_right = roll_man-500;
servo_left = 2500-roll_man;
servo_left = servo_left+(pitch_man-1500);
servo_right = servo_right+(pitch_man-1500);
servo_left = 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;
case 2:
servo_left = roll_man;
servo_right = pitch_man;
break;
}
else {
if (control_mode == 1) { servo_left = 1000; servo_right = 1000; }
else { servo_left = 1500; servo_right = 1500; }
}
break;
case 9:
case 10:
case 5:
if (autopilot_mode == 0) {
servo_right = steer_auto;
servo_left = 3000-steer_auto;
}
else if (autopilot_mode == 1) {
servo_right = steer_auto-500;
servo_left = 2500-steer_auto;
servo_left = constrain(servo_left, servo_auto_min, servo_auto_max);
if (servo_left>(servo_auto_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_auto_min+10)) {
servo_right = map(servo_right, 1000, 2000, servo_right_start, 2000);
}
else {
servo_right = servo_right_max;
}
}
servo_right = steer_auto;
servo_left = 3000-steer_auto;
break;
case 2:
case 3:
case 4:
servo_left = 1000;
servo_right = 1000;
break;
}
// ---------- Stage 2 - Linear mode ---------- //
switch (linear_mode) {
case 0: // control is fully linear
break;
case 1: // control start at servo_start with an offset
if (servo_left>(1500+trig)) {
servo_left = map(servo_left, 1500, 2000, 1500+left_offset, 2000);
}
else if (servo_left<(1500-trig)) {
servo_left = map(servo_left, 1500, 1000, 1500-left_offset, 1000);
}
if (servo_right>(1500+trig)) {
servo_right = map(servo_right, 1500, 2000, 1500+right_offset, 2000);
}
else if (servo_left<(1500-trig)) {
servo_right = map(servo_right, 1500, 1000, 1500-right_offset, 1000);
}
break;
}
// ---------- Stage 3 - Control mode ---------- //
switch (control_mode) {
case 0: // neutral is center
break;
case 1: // neutral is hands up
servo_right = constrain(map(servo_right, 1500, 2000, 1000, 2000), 1000, 2000);
servo_left = constrain(map(servo_left, 1500, 2000, 1000, 2000), 1000, 2000);
break;
}
// -------------------------- Deployment Servo and ESC -------------------------- //
switch(flight_mode) {
@ -1236,14 +1216,13 @@ void userinter() {
void setled(int a, int b, int c, int d, int e) {
if (batt_critical == true) { a = 255; b = 0; c = 0; d = 25; e = 50; }
if (batt_low == true) { a = 255; b = 0; c = 0; d = 25; e = 200; }
if ((millis()-lastled)>=e) {
lastled = millis();
colorWipe(strip.Color(a,b,c), 0);
if (led_model == 1) { colorWipe(strip.Color(b,a,c), 0); }
else { colorWipe(strip.Color(a,b,c), 0); }
duration = d;
timeled = millis();
}
@ -1446,6 +1425,9 @@ void sim() {
sim_cmd_time = (millis()*(PI/10000));
sim_cmd = sin(sim_cmd_time);
sim_cmd = map(sim_cmd, -1, 1, -180, 180);
if (test_dir_rc) {
sim_cmd = map(roll_man, 1000, 2000, -180, 180);
}
}
void eppclear() {
@ -1455,7 +1437,7 @@ void eppclear() {
void getconfig() {
if (!SD.begin(chipSelect)) { sd_ok = false; }
if (!SD.begin(chipSelect)) { sd_ok = false; delay(1500); }
else {
sd_ok = true;
File configFile = SD.open("config.txt", FILE_READ);
@ -1498,6 +1480,9 @@ void getconfig() {
last_waypoint_number = i+1;
configFile.close();
}
else {
delay(1500) ;
}
}
}