API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Apr 29th, 2017
164
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 46.07 KB | None | 0 0
raw download clone embed print report
  1. #include <Servo.h>
  2.  
  3. //#include <ServoTimer2.h>
  4.  
  5. #include <Wire.h>                          //Include the Wire.h library so we can communicate with the gyro.
  6. #include <EEPROM.h>                        //Include the EEPROM.h library so we can store information onto the EEPROM
  7.  
  8.  
  9. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  10. //PID gain and limit settings
  11. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  12. float pid_p_gain_roll = 1.4;               //Gain setting for the roll P-controller (1.3)
  13. float pid_i_gain_roll = 0.05;              //Gain setting for the roll I-controller (0.05)
  14. float pid_d_gain_roll = 15;                //Gain setting for the roll D-controller (15)
  15. int pid_max_roll = 400;                    //Maximum output of the PID-controller (+/-)
  16.  
  17. float pid_p_gain_pitch = pid_p_gain_roll;  //Gain setting for the pitch P-controller.
  18. float pid_i_gain_pitch = pid_i_gain_roll;  //Gain setting for the pitch I-controller.
  19. float pid_d_gain_pitch = pid_d_gain_roll;  //Gain setting for the pitch D-controller.
  20. int pid_max_pitch = pid_max_roll;          //Maximum output of the PID-controller (+/-)
  21.  
  22. float pid_p_gain_yaw = 4.0;                //Gain setting for the pitch P-controller. //4.0
  23. float pid_i_gain_yaw = 0.02;               //Gain setting for the pitch I-controller. //0.02
  24. float pid_d_gain_yaw = 0.0;                //Gain setting for the pitch D-controller.
  25. int pid_max_yaw = 400;                     //Maximum output of the PID-controller (+/-)
  26.  
  27. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  28. //Declaring global variables
  29. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  30. byte last_channel_1, last_channel_2, last_channel_3, last_channel_4;
  31. byte eeprom_data[36];
  32. byte highByte, lowByte;
  33. int receiver_input_channel_1, receiver_input_channel_2, receiver_input_channel_3, receiver_input_channel_4;
  34. int counter_channel_1, counter_channel_2, counter_channel_3, counter_channel_4, loop_counter;
  35. int esc_1, esc_2, esc_3, esc_4, servo_1, servo_2, servo_3, servo_4; //Added Servo
  36. int throttle, battery_voltage;
  37. int cal_int, start, gyro_address;
  38. int receiver_input[5];
  39. unsigned long timer_channel_1, timer_channel_2, timer_channel_3, timer_channel_4, esc_timer, esc_loop_timer;
  40. unsigned long timer_1, timer_2, timer_3, timer_4, current_time;
  41. unsigned long loop_timer;
  42. double gyro_pitch, gyro_roll, gyro_yaw;
  43. double gyro_axis[4], gyro_axis_cal[4];
  44. float pid_error_temp;
  45. float pid_i_mem_roll, pid_roll_setpoint, gyro_roll_input, pid_output_roll, pid_last_roll_d_error;
  46. float pid_i_mem_pitch, pid_pitch_setpoint, gyro_pitch_input, pid_output_pitch, pid_last_pitch_d_error;
  47. float pid_i_mem_yaw, pid_yaw_setpoint, gyro_yaw_input, pid_output_yaw, pid_last_yaw_d_error;
  48.  
  49. /////////////////////////////
  50. // Legger inn Calibration values:
  51. //////////////////////////////
  52. int s1 = 1500;
  53. int s2 = 1500;
  54. int s3 = 1500;
  55. int s4 = 1500;
  56.  
  57. int s_inc = 400;
  58. int s1_max, s2_max, s3_max, s4_max;
  59.  
  60. unsigned long tid, tid2;
  61. char data_in;
  62. bool cal = false;
  63. int servo_value;
  64.  
  65. int number = 0;
  66. bool debug = true;
  67.  
  68. void calibrate_servos();
  69.  
  70. // Servo
  71. Servo servo1;
  72. Servo servo2;
  73. Servo servo3;
  74. Servo servo4;
  75.  
  76. const float h[] = { 0.0151963734917116, 0.125956465856097,  0.358847160652191,  0.358847160652191,  0.125956465856097,  0.0151963734917116};
  77.  
  78. #define M (sizeof(h)/sizeof(float))        // Defining length of shift register
  79. float x[M] = {0};                          // Setting all values in shiftregister B as zero value
  80. int N = M - 1;                             // Filter Order
  81. float pitch_angle_lowpass = 0;             // Output variable for y[n]
  82.  
  83. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  84. //Setup routine
  85. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  86. void setup() {
  87.  
  88.   DDRA |= B11110000;
  89.   DDRE |= B00110000;
  90.   DDRB |= B11000000;
  91.  
  92.   //pinMode(A3, OUTPUT);
  93.   Serial.begin(57600);
  94.   Serial.println("starting");
  95.   //Read EEPROM for fast access data.
  96.   for (start = 0; start <= 35; start++)eeprom_data[start] = EEPROM.read(start);
  97.   gyro_address = eeprom_data[32];                              //Store the gyro address in the variable.
  98.  
  99.  servo1.attach(2); // portE PE4 B00010000
  100.   servo2.attach(3); // portE PE5 B00100000
  101.   servo3.attach(12);// portE PE6 B01000000
  102.   servo4.attach(13);// portE Pe6 B10000000
  103.  
  104.   Wire.begin();                                                //Start the I2C as master.
  105.  
  106.  
  107.  
  108.   //Use the led on the Arduino for startup indication.
  109.   //digitalWrite(A2, HIGH);                                      //Turn on the warning led.
  110.  
  111.   //Check the EEPROM signature to make sure that the setup program is executed
  112.   while (eeprom_data[33] != 'J' || eeprom_data[34] != 'M' || eeprom_data[35] != 'B')delay(10);
  113.  
  114.   set_gyro_registers();                                        //Set the specific gyro registers.
  115.  
  116.   for (cal_int = 0; cal_int < 1250 ; cal_int ++) {             //Wait 5 seconds before continuing.
  117.     PORTA |= B11110000;                                        //Set digital poort 29, 28, 27, 26 high.
  118.     delayMicroseconds(1000);                                   //Wait 1000us.
  119.     PORTA &= B00001111;                                        //Set digital poort 29, 28, 27, 26 low.
  120.     //PORTA &= B00000000;                                     // Sjekk dette
  121.     delayMicroseconds(3000);                                   //Wait 3000us.
  122.   }
  123.  
  124.   //Let's take multiple gyro data samples so we can determine the average gyro offset (calibration).
  125.   for (cal_int = 0; cal_int < 2000 ; cal_int ++) {             //Take 2000 readings for calibration.
  126.     //  if (cal_int % 15 == 0)  //Change the led status to indicate calibration.
  127.     gyro_signalen();                                           //Read the gyro output.
  128.     gyro_axis_cal[1] += gyro_axis[1];                          //Ad roll value to gyro_roll_cal.
  129.     gyro_axis_cal[2] += gyro_axis[2];                          //Ad pitch value to gyro_pitch_cal.
  130.     gyro_axis_cal[3] += gyro_axis[3];                          //Ad yaw value to gyro_yaw_cal.
  131.     //We don't want the esc's to be beeping annoyingly. So let's give them a 1000us puls while calibrating the gyro.
  132.     PORTA |= B11110000;                                        //Set digital poort 29, 28, 27, 26 high.
  133.     delayMicroseconds(1000);                                   //Wait 1000us.
  134.     PORTA &= B00001111;                                        //Set digital poort 29, 28, 27, 26 low.
  135.     delay(3);                                                  //Wait 3 milliseconds before the next loop.
  136.   }
  137.   //Now that we have 2000 measures, we need to devide by 2000 to get the average gyro offset.
  138.   gyro_axis_cal[1] /= 2000;                                    //Divide the roll total by 2000.
  139.   gyro_axis_cal[2] /= 2000;                                    //Divide the pitch total by 2000.
  140.   gyro_axis_cal[3] /= 2000;                                    //Divide the yaw total by 2000.
  141.  
  142.   PCICR |= (1 << PCIE0);                                       //Set PCIE0 to enable PCMSK0 scan.
  143.   PCMSK0 |= (1 << PCINT0);                                     //Set PCINT0 (digital input 53) to trigger an interrupt on state change.
  144.   PCMSK0 |= (1 << PCINT1);                                     //Set PCINT1 (digital input 52)to trigger an interrupt on state change.
  145.   PCMSK0 |= (1 << PCINT2);                                     //Set PCINT2 (digital input 51)to trigger an interrupt on state change.
  146.   PCMSK0 |= (1 << PCINT3);                                     //Set PCINT3 (digital input 50)to trigger an interrupt on state change.
  147.  
  148.   //Wait until the receiver is active and the throtle is set to the lower position.
  149.   while (receiver_input_channel_3 < 990 || receiver_input_channel_3 > 1020 || receiver_input_channel_4 < 1400) {
  150.     receiver_input_channel_3 = convert_receiver_channel(3);    //Convert the actual receiver signals for throttle to the standard 1000 - 2000us
  151.     receiver_input_channel_4 = convert_receiver_channel(4);    //Convert the actual receiver signals for yaw to the standard 1000 - 2000us
  152.     start ++;                                                  //While waiting increment start whith every loop.
  153.     //We don't want the esc's to be beeping annoyingly. So let's give them a 1000us puls while waiting for the receiver inputs.
  154.     PORTA |= B11110000;                                        //Set digital poort port 29, 28, 27, 26 high.
  155.     delayMicroseconds(1000);                                   //Wait 1000us.
  156.     PORTA &= B00001111;                                        //Set digital poort 29, 28, 27, 26 low.
  157.     delay(3);                                                  //Wait 3 milliseconds before the next loop.
  158.     //if (start == 125) {                                        //Every 125 loops (500ms).
  159.     //digitalWrite(A2, !digitalRead(A2));                      //Change the led status.
  160.     //  start = 0;                                               //Start again at 0.
  161.     //}
  162.   }
  163.   start = 0;                                                   //Set start back to 0.
  164.  
  165.   //Load the battery voltage to the battery_voltage variable.
  166.   //65 is the voltage compensation for the diode.
  167.   //12.6V equals ~5V @ Analog 0.
  168.   //12.6V equals 1023 analogRead(0).
  169.   //1260 / 1023 = 1.2317.
  170.   //The variable battery_voltage holds 1050 if the battery voltage is 10.5V.
  171.   // battery_voltage = (analogRead(0) + 65) * 1.2317;
  172.   //  pinMode(2, OUTPUT);
  173.   //  pinMode(3, OUTPUT);
  174.   //  pinMode(12, OUTPUT);
  175.   //  pinMode(13, OUTPUT);
  176.   //
  177.  
  178.  
  179.   servo1.writeMicroseconds(s1);
  180.   servo2.writeMicroseconds(s2);
  181.   servo3.writeMicroseconds(s3);
  182.   servo4.writeMicroseconds(s4);
  183.  
  184.   calibrate_servos();
  185.  
  186.   s1_max = s1 + s_inc;
  187.   s2_max = s2 - s_inc;
  188.   s3_max = s3 + s_inc;
  189.   s4_max = s4 - s_inc;
  190.  
  191.   servo1.writeMicroseconds(s1);
  192.   servo2.writeMicroseconds(s2);
  193.   servo3.writeMicroseconds(s3);
  194.   servo4.writeMicroseconds(s4);
  195.  
  196.  
  197.   //Arduino (Atmega) pins default to inputs, so they don't need to be explicitly declared as inputs.
  198.   //Configure digital poort 29, 28, 27, 26.
  199.  
  200. }
  201. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  202. //Main program loop
  203. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  204. void loop() {
  205.  
  206.   receiver_input_channel_1 = convert_receiver_channel(1);      //Convert the actual receiver signals for pitch to the standard 1000 - 2000us.
  207.   receiver_input_channel_2 = convert_receiver_channel(2);      //Convert the actual receiver signals for roll to the standard 1000 - 2000us.
  208.   receiver_input_channel_3 = convert_receiver_channel(3);      //Convert the actual receiver signals for throttle to the standard 1000 - 2000us.
  209.   receiver_input_channel_4 = convert_receiver_channel(4);      //Convert the actual receiver signals for yaw to the standard 1000 - 2000us.
  210.  
  211.   //Let's get the current gyro data and scale it to degrees per second for the pid calculations.
  212.   gyro_signalen();
  213.  
  214.   gyro_roll_input = (gyro_roll_input * 0.8) + ((gyro_roll / 57.14286) * 0.2);            //Gyro pid input is deg/sec.
  215.   gyro_pitch_input = (gyro_pitch_input * 0.8) + ((gyro_pitch / 57.14286) * 0.2);         //Gyro pid input is deg/sec.
  216.   gyro_yaw_input = (gyro_yaw_input * 0.8) + ((gyro_yaw / 57.14286) * 0.2);               //Gyro pid input is deg/sec.
  217.   /*
  218.     /////////////////////////////
  219.     //NEW FILTER TEST
  220.     /////////////////////////////
  221.     int i = 0;
  222.     for(i=N; i>0; i--) {    // Shifts all previous samples one position
  223.     x[i] = x[i-1];        // Makes room for one new sample
  224.     }
  225.  
  226.     x[0] = gyro_pitch_input;
  227.     pitch_angle_lowpass = h[0] * x[0];   // Compute the convolution x[0]*h[0]7
  228.  
  229.     for(i = 1; i <= N; i++) {            // Summing the rest of the products
  230.     pitch_angle_lowpass += h[i] * x[i]; // Convolve rest of the inputs with the filter coefficients
  231.     }
  232.   */
  233.   //For starting the motors: throttle low and yaw left (step 1).
  234.   if (receiver_input_channel_3 < 1050 && receiver_input_channel_4 < 1050)start = 1;
  235.   //When yaw stick is back in the center position start the motors (step 2).
  236.   if (start == 1 && receiver_input_channel_3 < 1050 && receiver_input_channel_4 > 1450) {
  237.     start = 2;
  238.     //Reset the pid controllers for a bumpless start.
  239.     pid_i_mem_roll = 0;
  240.     pid_last_roll_d_error = 0;
  241.     pid_i_mem_pitch = 0;
  242.     pid_last_pitch_d_error = 0;
  243.     pid_i_mem_yaw = 0;
  244.     pid_last_yaw_d_error = 0;
  245.   }
  246.   //Stopping the motors: throttle low and yaw right.
  247.   if (start == 2 && receiver_input_channel_3 < 1050 && receiver_input_channel_4 > 1950)start = 0;
  248.  
  249.   //The PID set point in degrees per second is determined by the roll receiver input.
  250.   //In the case of deviding by 3 the max roll rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ).
  251.   pid_roll_setpoint = 0;
  252.   //We need a little dead band of 16us for better results.
  253.   if (receiver_input_channel_1 > 1508)pid_roll_setpoint = (receiver_input_channel_1 - 1508) / 3.0;
  254.   else if (receiver_input_channel_1 < 1492)pid_roll_setpoint = (receiver_input_channel_1 - 1492) / 3.0;
  255.  
  256.   //The PID set point in degrees per second is determined by the pitch receiver input.
  257.   //In the case of deviding by 3 the max pitch rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ).
  258.   pid_pitch_setpoint = 0;
  259.   //We need a little dead band of 16us for better results.
  260.   if (receiver_input_channel_2 > 1508)pid_pitch_setpoint = (receiver_input_channel_2 - 1508) / 3.0;
  261.   else if (receiver_input_channel_2 < 1492)pid_pitch_setpoint = (receiver_input_channel_2 - 1492) / 3.0;
  262.  
  263.   //The PID set point in degrees per second is determined by the yaw receiver input.
  264.   //In the case of deviding by 3 the max yaw rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ).
  265.   pid_yaw_setpoint = 0;
  266.   //We need a little dead band of 16us for better results.
  267.   if (receiver_input_channel_3 > 1050) { //Do not yaw when turning off the motors.
  268.     if (receiver_input_channel_4 > 1508)pid_yaw_setpoint = (receiver_input_channel_4 - 1508) / 3.0;
  269.     else if (receiver_input_channel_4 < 1492)pid_yaw_setpoint = (receiver_input_channel_4 - 1492) / 3.0;
  270.   }
  271.   //PID inputs are known. So we can calculate the pid output.
  272.   calculate_pid();
  273.  
  274.   //The battery voltage is needed for compensation.
  275.   //A complementary filter is used to reduce noise.
  276.   //0.09853 = 0.08 * 1.2317.
  277.   //battery_voltage = battery_voltage * 0.92 + (analogRead(0) + 65) * 0.09853;
  278.  
  279.   //Turn on the led if battery voltage is to low.
  280.   //if (battery_voltage < 1030 && battery_voltage > 600)digitalWrite(A2, HIGH);
  281.  
  282.   throttle = receiver_input_channel_3;                                      //We need the throttle signal as a base signal.
  283.  
  284.   if (start == 2) {                                                         //The motors are started.
  285.     if (throttle > 1800) throttle = 1800;                                   //We need some room to keep full control at full throttle.
  286.  
  287.     esc_1 = throttle - pid_output_pitch + pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 1 (front-right - CCW)
  288.     esc_2 = throttle + pid_output_pitch + pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 2 (rear-right - CW)
  289.     esc_3 = throttle + pid_output_pitch - pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 3 (rear-left - CCW)
  290.     esc_4 = throttle - pid_output_pitch - pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 4 (front-left - CW)
  291.  
  292.     /*if (battery_voltage < 1240 && battery_voltage > 800) {                  //Is the battery connected?
  293.       esc_1 += esc_1 * ((1240 - battery_voltage) / (float)3500);            //Compensate the esc-1 pulse for voltage drop.
  294.       esc_2 += esc_2 * ((1240 - battery_voltage) / (float)3500);            //Compensate the esc-2 pulse for voltage drop.
  295.       esc_3 += esc_3 * ((1240 - battery_voltage) / (float)3500);            //Compensate the esc-3 pulse for voltage drop.
  296.       esc_4 += esc_4 * ((1240 - battery_voltage) / (float)3500);            //Compensate the esc-4 pulse for voltage drop.
  297.       }*/
  298.  
  299.     servo_1 = map(esc_1, 1000, 2000, s1, s1_max); //Problem???????????????????????????????????????????????????????????????????????
  300.     servo_2 = map(esc_2, 1000, 2000, s2, s2_max);
  301.     servo_3 = map(esc_3, 1000, 2000, s3, s3_max);
  302.     servo_4 = map(esc_4, 1000, 2000, s4, s4_max); //what is the middle value 1500 throttle? (0 deg -> +7 deg, find the values)
  303.  
  304.     //send servo val <--------------------------------------------------------------------------------------------------------------------<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
  305.     servo1.writeMicroseconds(servo_1);
  306.     servo2.writeMicroseconds(servo_2);
  307.     servo3.writeMicroseconds(servo_3);
  308.     servo4.writeMicroseconds(servo_4);
  309.     if (debug) {
  310.       Serial.print(number);
  311.       number++;
  312.       Serial.print(" ");
  313.       Serial.print(throttle);
  314.       Serial.print(" ");
  315.       Serial.print(gyro_pitch_input );
  316.       Serial.print(" ");
  317.       Serial.print(gyro_roll_input);
  318.       Serial.print(" ");
  319.       Serial.println(gyro_yaw_input);
  320.     }
  321.  
  322.     if (esc_1 < 1200) esc_1 = 1200;                                         //Keep the motors running.
  323.     if (esc_2 < 1200) esc_2 = 1200;                                         //Keep the motors running.
  324.     if (esc_3 < 1200) esc_3 = 1200;                                         //Keep the motors running.
  325.     if (esc_4 < 1200) esc_4 = 1200;                                         //Keep the motors running.
  326.  
  327.     if (esc_1 > 2000)esc_1 = 2000;                                          //Limit the esc-1 pulse to 2000us.
  328.     if (esc_2 > 2000)esc_2 = 2000;                                          //Limit the esc-2 pulse to 2000us.
  329.     if (esc_3 > 2000)esc_3 = 2000;                                          //Limit the esc-3 pulse to 2000us.
  330.     if (esc_4 > 2000)esc_4 = 2000;                                          //Limit the esc-4 pulse to 2000us.
  331.   }
  332.  
  333.   else {
  334.     esc_1 = 1000;                                                           //If start is not 2 keep a 1000us pulse for ess-1.
  335.     esc_2 = 1000;                                                           //If start is not 2 keep a 1000us pulse for ess-2.
  336.     esc_3 = 1000;                                                           //If start is not 2 keep a 1000us pulse for ess-3.
  337.     esc_4 = 1000;                                                           //If start is not 2 keep a 1000us pulse for ess-4.
  338.     servo1.writeMicroseconds(s1);
  339.     servo2.writeMicroseconds(s2);
  340.     servo3.writeMicroseconds(s3);
  341.     servo4.writeMicroseconds(s4);
  342.   }
  343.  
  344.   //All the information for controlling the motor's is available.
  345.   //The refresh rate is 250Hz. That means the esc's need there pulse every 4ms.
  346.   while (micros() - loop_timer < 4000);//We wait until 4000us are passed.
  347.   loop_timer = micros();                                                    //Set the timer for the next loop.
  348.  
  349.   PORTA |= B11110000;                                                       //Set digital outputs 29, 28, 27, 26 high.
  350.   timer_channel_1 = esc_1 + loop_timer;                                     //Calculate the time of the faling edge of the esc-1 pulse.
  351.   timer_channel_2 = esc_2 + loop_timer;                                     //Calculate the time of the faling edge of the esc-2 pulse.
  352.   timer_channel_3 = esc_3 + loop_timer;                                     //Calculate the time of the faling edge of the esc-3 pulse.
  353.   timer_channel_4 = esc_4 + loop_timer;                                     //Calculate the time of the faling edge of the esc-4 pulse.
  354.  
  355.   while (PORTA >= 16) {                                                     //Stay in this loop until output are low.
  356.     esc_loop_timer = micros();                                              //Read the current time.
  357.     if (timer_channel_1 <= esc_loop_timer)PORTA &= B11101111;               //Set digital output 26 to low if the time is expired.
  358.     if (timer_channel_2 <= esc_loop_timer)PORTA &= B11011111;               //Set digital output 27 to low if the time is expired.
  359.     if (timer_channel_3 <= esc_loop_timer)PORTA &= B10111111;               //Set digital output 28 to low if the time is expired.
  360.     if (timer_channel_4 <= esc_loop_timer)PORTA &= B01111111;               //Set digital output 29 to low if the time is expired.
  361.   }
  362. }
  363.  
  364. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  365. //This routine is called every time input 8, 9, 10 or 11 changed state
  366. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  367. ISR(PCINT0_vect) {
  368.   current_time = micros();
  369.   //Channel 1=========================================
  370.   if (PINB & B00000001) {                                      //Is input 53 high?
  371.     if (last_channel_1 == 0) {                                 //Input 53 changed from 0 to 1
  372.       last_channel_1 = 1;                                      //Remember current input state
  373.       timer_1 = current_time;                                  //Set timer_1 to current_time
  374.     }
  375.   }
  376.   else if (last_channel_1 == 1) {                              //Input 53 is not high and changed from 1 to 0
  377.     last_channel_1 = 0;                                        //Remember current input state
  378.     receiver_input[1] = current_time - timer_1;                //Channel 1 is current_time - timer_1
  379.   }
  380.   //Channel 2=========================================
  381.   if (PINB & B00000010 ) {                                     //Is input 52 high?
  382.     if (last_channel_2 == 0) {                                 //Input 52 changed from 0 to 1
  383.       last_channel_2 = 1;                                      //Remember current input state
  384.       timer_2 = current_time;                                  //Set timer_2 to current_time
  385.     }
  386.   }
  387.   else if (last_channel_2 == 1) {                              //Input 52 is not high and changed from 1 to 0
  388.     last_channel_2 = 0;                                        //Remember current input state
  389.     receiver_input[2] = current_time - timer_2;                //Channel 2 is current_time - timer_2
  390.   }
  391.   //Channel 3=========================================
  392.   if (PINB & B00000100 ) {                                     //Is input 51 high?
  393.     if (last_channel_3 == 0) {                                 //Input 51 changed from 0 to 1
  394.       last_channel_3 = 1;                                      //Remember current input state
  395.       timer_3 = current_time;                                  //Set timer_3 to current_time
  396.     }
  397.   }
  398.   else if (last_channel_3 == 1) {                              //Input 51 is not high and changed from 1 to 0
  399.     last_channel_3 = 0;                                        //Remember current input state
  400.     receiver_input[3] = current_time - timer_3;                //Channel 3 is current_time - timer_3
  401.  
  402.   }
  403.   //Channel 4=========================================
  404.   if (PINB & B00001000 ) {                                     //Is input 50  high?
  405.     if (last_channel_4 == 0) {                                 //Input 50 changed from 0 to 1
  406.       last_channel_4 = 1;                                      //Remember current input state
  407.       timer_4 = current_time;                                  //Set timer_4 to current_time
  408.     }
  409.   }
  410.   else if (last_channel_4 == 1) {                              //Input 50 is not high and changed from 1 to 0
  411.     last_channel_4 = 0;                                        //Remember current input state
  412.     receiver_input[4] = current_time - timer_4;                //Channel 4 is current_time - timer_4
  413.   }
  414. }
  415.  
  416. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  417. //Subroutine for reading the gyro
  418. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  419. void gyro_signalen() {
  420.   //Read the L3G4200D or L3GD20H
  421.   if (eeprom_data[31] == 2 || eeprom_data[31] == 3) {
  422.     Wire.beginTransmission(gyro_address);                        //Start communication with the gyro (adress 1101001)
  423.     Wire.write(168);                                             //Start reading @ register 28h and auto increment with every read
  424.     Wire.endTransmission();                                      //End the transmission
  425.     Wire.requestFrom(gyro_address, 6);                           //Request 6 bytes from the gyro
  426.     while (Wire.available() < 6);                                //Wait until the 6 bytes are received
  427.     lowByte = Wire.read();                                       //First received byte is the low part of the angular data
  428.     highByte = Wire.read();                                      //Second received byte is the high part of the angular data
  429.     gyro_axis[1] = ((highByte << 8) | lowByte);                  //Multiply highByte by 256 (shift left by 8) and ad lowByte
  430.     lowByte = Wire.read();                                       //First received byte is the low part of the angular data
  431.     highByte = Wire.read();                                      //Second received byte is the high part of the angular data
  432.     gyro_axis[2] = ((highByte << 8) | lowByte);                  //Multiply highByte by 256 (shift left by 8) and ad lowByte
  433.     lowByte = Wire.read();                                       //First received byte is the low part of the angular data
  434.     highByte = Wire.read();                                      //Second received byte is the high part of the angular data
  435.     gyro_axis[3] = ((highByte << 8) | lowByte);                  //Multiply highByte by 256 (shift left by 8) and ad lowByte
  436.   }
  437.  
  438.   //Read the MPU-6050
  439.   if (eeprom_data[31] == 1) {
  440.     Wire.beginTransmission(gyro_address);                        //Start communication with the gyro
  441.     Wire.write(0x43);                                            //Start reading @ register 43h and auto increment with every read
  442.     Wire.endTransmission();                                      //End the transmission
  443.     Wire.requestFrom(gyro_address, 6);                           //Request 6 bytes from the gyro
  444.     while (Wire.available() < 6);                                //Wait until the 6 bytes are received
  445.     gyro_axis[1] = Wire.read() << 8 | Wire.read();               //Read high and low part of the angular data
  446.     gyro_axis[2] = Wire.read() << 8 | Wire.read();               //Read high and low part of the angular data
  447.     gyro_axis[3] = Wire.read() << 8 | Wire.read();               //Read high and low part of the angular data
  448.   }
  449.  
  450.   if (cal_int == 2000) {
  451.     gyro_axis[1] -= gyro_axis_cal[1];                            //Only compensate after the calibration
  452.     gyro_axis[2] -= gyro_axis_cal[2];                            //Only compensate after the calibration
  453.     gyro_axis[3] -= gyro_axis_cal[3];                            //Only compensate after the calibration
  454.   }
  455.   gyro_roll = gyro_axis[eeprom_data[28] & 0b00000011];
  456.   if (eeprom_data[28] & 0b10000000)gyro_roll *= -1;
  457.   gyro_pitch = gyro_axis[eeprom_data[29] & 0b00000011];
  458.   if (eeprom_data[29] & 0b10000000)gyro_pitch *= -1;
  459.   gyro_yaw = gyro_axis[eeprom_data[30] & 0b00000011];
  460.   if (eeprom_data[30] & 0b10000000)gyro_yaw *= -1;
  461.  
  462. }
  463.  
  464. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  465. //Subroutine for calculating pid outputs
  466. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  467. //The PID controllers are explained in part 5 of the YMFC-3D video session:
  468. //www.youtube.com/watch?v=JBvnB0279-Q
  469.  
  470. void calculate_pid() {
  471.   //Roll calculations
  472.   pid_error_temp = gyro_roll_input - pid_roll_setpoint;
  473.   pid_i_mem_roll += pid_i_gain_roll * pid_error_temp;
  474.   if (pid_i_mem_roll > pid_max_roll)pid_i_mem_roll = pid_max_roll;
  475.   else if (pid_i_mem_roll < pid_max_roll * -1)pid_i_mem_roll = pid_max_roll * -1;
  476.  
  477.   pid_output_roll = pid_p_gain_roll * pid_error_temp + pid_i_mem_roll + pid_d_gain_roll * (pid_error_temp - pid_last_roll_d_error);
  478.   if (pid_output_roll > pid_max_roll)pid_output_roll = pid_max_roll;
  479.   else if (pid_output_roll < pid_max_roll * -1)pid_output_roll = pid_max_roll * -1;
  480.  
  481.   pid_last_roll_d_error = pid_error_temp;
  482.  
  483.   //Pitch calculations
  484.   pid_error_temp = gyro_pitch_input - pid_pitch_setpoint;
  485.   pid_i_mem_pitch += pid_i_gain_pitch * pid_error_temp;
  486.   if (pid_i_mem_pitch > pid_max_pitch)pid_i_mem_pitch = pid_max_pitch;
  487.   else if (pid_i_mem_pitch < pid_max_pitch * -1)pid_i_mem_pitch = pid_max_pitch * -1;
  488.  
  489.   pid_output_pitch = pid_p_gain_pitch * pid_error_temp + pid_i_mem_pitch + pid_d_gain_pitch * (pid_error_temp - pid_last_pitch_d_error);
  490.   if (pid_output_pitch > pid_max_pitch)pid_output_pitch = pid_max_pitch;
  491.   else if (pid_output_pitch < pid_max_pitch * -1)pid_output_pitch = pid_max_pitch * -1;
  492.  
  493.   pid_last_pitch_d_error = pid_error_temp;
  494.  
  495.   //Yaw calculations
  496.   pid_error_temp = gyro_yaw_input - pid_yaw_setpoint;
  497.   pid_i_mem_yaw += pid_i_gain_yaw * pid_error_temp;
  498.   if (pid_i_mem_yaw > pid_max_yaw)pid_i_mem_yaw = pid_max_yaw;
  499.   else if (pid_i_mem_yaw < pid_max_yaw * -1)pid_i_mem_yaw = pid_max_yaw * -1;
  500.  
  501.   pid_output_yaw = pid_p_gain_yaw * pid_error_temp + pid_i_mem_yaw + pid_d_gain_yaw * (pid_error_temp - pid_last_yaw_d_error);
  502.   if (pid_output_yaw > pid_max_yaw)pid_output_yaw = pid_max_yaw;
  503.   else if (pid_output_yaw < pid_max_yaw * -1)pid_output_yaw = pid_max_yaw * -1;
  504.  
  505.   pid_last_yaw_d_error = pid_error_temp;
  506. }
  507.  
  508. //This part converts the actual receiver signals to a standardized 1000 – 1500 – 2000 microsecond value.
  509. //The stored data in the EEPROM is used.
  510. int convert_receiver_channel(byte function) {
  511.   byte channel, reverse;                                                       //First we declare some local variables
  512.   int low, center, high, actual;
  513.   int difference;
  514.  
  515.   channel = eeprom_data[function + 23] & 0b00000111;                           //What channel corresponds with the specific function
  516.   if (eeprom_data[function + 23] & 0b10000000)reverse = 1;                     //Reverse channel when most significant bit is set
  517.   else reverse = 0;                                                            //If the most significant is not set there is no reverse
  518.  
  519.   actual = receiver_input[channel];                                            //Read the actual receiver value for the corresponding function
  520.   low = (eeprom_data[channel * 2 + 15] << 8) | eeprom_data[channel * 2 + 14];  //Store the low value for the specific receiver input channel
  521.   center = (eeprom_data[channel * 2 - 1] << 8) | eeprom_data[channel * 2 - 2]; //Store the center value for the specific receiver input channel
  522.   high = (eeprom_data[channel * 2 + 7] << 8) | eeprom_data[channel * 2 + 6];   //Store the high value for the specific receiver input channel
  523.  
  524.   if (actual < center) {                                                       //The actual receiver value is lower than the center value
  525.     if (actual < low)actual = low;                                             //Limit the lowest value to the value that was detected during setup
  526.     difference = ((long)(center - actual) * (long)500) / (center - low);       //Calculate and scale the actual value to a 1000 - 2000us value
  527.     if (reverse == 1)return 1500 + difference;                                 //If the channel is reversed
  528.     else return 1500 - difference;                                             //If the channel is not reversed
  529.   }
  530.   else if (actual > center) {                                                                      //The actual receiver value is higher than the center value
  531.     if (actual > high)actual = high;                                           //Limit the lowest value to the value that was detected during setup
  532.     difference = ((long)(actual - center) * (long)500) / (high - center);      //Calculate and scale the actual value to a 1000 - 2000us value
  533.     if (reverse == 1)return 1500 - difference;                                 //If the channel is reversed
  534.     else return 1500 + difference;                                             //If the channel is not reversed
  535.   }
  536.   else return 1500;
  537. }
  538.  
  539. void set_gyro_registers() {
  540.   //Setup the MPU-6050
  541.   if (eeprom_data[31] == 1) {
  542.     Wire.beginTransmission(gyro_address);                        //Start communication with the address found during search.
  543.     Wire.write(0x6B);                                            //We want to write to the PWR_MGMT_1 register (6B hex)
  544.     Wire.write(0x00);                                            //Set the register bits as 00000000 to activate the gyro
  545.     Wire.endTransmission();                                      //End the transmission with the gyro.
  546.  
  547.     Wire.beginTransmission(gyro_address);                        //Start communication with the address found during search.
  548.     Wire.write(0x1B);                                            //We want to write to the GYRO_CONFIG register (1B hex)
  549.     Wire.write(0x08);                                            //Set the register bits as 00001000 (500dps full scale)
  550.     Wire.endTransmission();                                      //End the transmission with the gyro
  551.  
  552.     //Let's perform a random register check to see if the values are written correct
  553.     Wire.beginTransmission(gyro_address);                        //Start communication with the address found during search
  554.     Wire.write(0x1B);                                            //Start reading @ register 0x1B
  555.     Wire.endTransmission();                                      //End the transmission
  556.     Wire.requestFrom(gyro_address, 1);                           //Request 1 bytes from the gyro
  557.     while (Wire.available() < 1);                                //Wait until the 6 bytes are received
  558.     if (Wire.read() != 0x08) {                                   //Check if the value is 0x08
  559.       //digitalWrite(A2, HIGH);                                    //Turn on the warning led
  560.       while (1)delay(10);                                        //Stay in this loop for ever
  561.     }
  562.  
  563.     Wire.beginTransmission(gyro_address);                        //Start communication with the address found during search
  564.     Wire.write(0x1A);                                            //We want to write to the GYRO_CONFIG register (1B hex)
  565.     Wire.write(0x03);                                            //Set the register bits as 00001000 (500dps full scale)
  566.     Wire.endTransmission();                                      //End the transmission with the gyro
  567.   }
  568.   //Setup the L3G4200D
  569.   if (eeprom_data[31] == 2) {
  570.     Wire.beginTransmission(gyro_address);                        //Start communication with the address found during search.
  571.     Wire.write(0x20);                                            //We want to write to register 1 (20 hex).
  572.     Wire.write(0x0F);                                            //Set the register bits as 00001111 (Turn on the gyro and enable all axis).
  573.     Wire.endTransmission();                                      //End the transmission with the gyro.
  574.  
  575.     Wire.beginTransmission(gyro_address);                        //Start communication with the address found during search.
  576.     Wire.write(0x23);                                            //We want to write to register 4 (23 hex).
  577.     Wire.write(0x90);                                            //Set the register bits as 10010000 (Block Data Update active & 500dps full scale).
  578.     Wire.endTransmission();                                      //End the transmission with the gyro.
  579.  
  580.     //Let's perform a random register check to see if the values are written correct
  581.     Wire.beginTransmission(gyro_address);                        //Start communication with the address found during search
  582.     Wire.write(0x23);                                            //Start reading @ register 0x23
  583.     Wire.endTransmission();                                      //End the transmission
  584.     Wire.requestFrom(gyro_address, 1);                           //Request 1 bytes from the gyro
  585.     while (Wire.available() < 1);                                //Wait until the 6 bytes are received
  586.     if (Wire.read() != 0x90) {                                   //Check if the value is 0x90
  587.       //digitalWrite(A2, HIGH);                                    //Turn on the warning led
  588.       while (1)delay(10);                                        //Stay in this loop for ever
  589.     }
  590.  
  591.   }
  592.   //Setup the L3GD20H
  593.   if (eeprom_data[31] == 3) {
  594.     Wire.beginTransmission(gyro_address);                        //Start communicationwith the address found during search.
  595.     Wire.write(0x20);                                            //We want to write to register 1 (20 hex).
  596.     Wire.write(0x0F);                                            //Set the register bits as 00001111 (Turn on the gyro and enable all axis).
  597.     Wire.endTransmission();                                      //End the transmission with the gyro.
  598.  
  599.     Wire.beginTransmission(gyro_address);                        //Start communication with the address found during search.
  600.     Wire.write(0x23);                                            //We want to write to register 4 (23 hex).
  601.     Wire.write(0x90);                                            //Set the register bits as 10010000 (Block Data Update active & 500dps full scale).
  602.     Wire.endTransmission();                                      //End the transmission with the gyro.
  603.  
  604.     //Let's perform a random register check to see if the values are written correct
  605.     Wire.beginTransmission(gyro_address);                        //Start communication with the address found during search
  606.     Wire.write(0x23);                                            //Start reading @ register 0x23
  607.     Wire.endTransmission();                                      //End the transmission
  608.     Wire.requestFrom(gyro_address, 1);                           //Request 1 bytes from the gyro
  609.     while (Wire.available() < 1);                                //Wait until the 6 bytes are received
  610.     if (Wire.read() != 0x90) {                                   //Check if the value is 0x90
  611.       //digitalWrite(A2, HIGH);                                    //Turn on the warning led
  612.       while (1)delay(10);                                        //Stay in this loop for ever
  613.     }
  614.   }
  615. }
  616.  
  617.  
  618.  
  619. /////////////////////////////////////////////
  620. // Calibration of Servos
  621. /////////////////////////////////////////////
  622. void calibrate_servos () {
  623.   tid = millis();
  624.  
  625.   int count_motor = 1;
  626.   /*
  627.      Vi vil hente inn info fra serial monitor og justere servoen derifra
  628.      Startvinkelen skal lagres som "null-vinkel" og overføres via EEPROM
  629.      Når vi skriver OK i Serial monitor skal vi hoppe videre til neste motor
  630.      Funksjonen skal gå igjennom kalibrering for alle fire motorene.
  631.   */
  632.   Serial.println("Skriv inn '*' for å komme til kalibrering");
  633.   if (!cal) {
  634.     while (tid + 7000 > tid2) {
  635.       tid2 = millis();
  636.       if (Serial.available()) {
  637.         data_in = Serial.read();
  638.         if (data_in == '*') {
  639.           cal = true;
  640.         }
  641.       }
  642.     }
  643.   }
  644.   if (cal) {
  645.     Serial.println("Velkommen til kalllllibrering");
  646.     delay(2000);
  647.     Serial.println("Start med motor 1: Skriv '+' for aa oeke vinkelen og '-' for aa minke den... Skriv OK naar du vil gaa til neste motor");
  648.     while (count_motor < 5) {
  649.  
  650.       servo_value = s1;
  651.       Serial.println("Sett servo1 til null-pitch og skriv OK naar du er ferdig med servo 1");
  652.       while (count_motor == 1) {
  653.         if (Serial.available()) {
  654.           data_in = Serial.read();
  655.           if (data_in == '0') {
  656.             s1 = servo_value;
  657.             count_motor++;
  658.           }
  659.           else {
  660.             if (data_in == '1') {
  661.               servo_value++;
  662.               servo1.writeMicroseconds(servo_value);
  663.               Serial.print("Servoverdi er nå : ");
  664.               Serial.println(servo_value);
  665.             }
  666.             else if (data_in == '2') {
  667.               servo_value--;
  668.               servo1.writeMicroseconds(servo_value);
  669.               Serial.print("Servoverdi er nå : " );
  670.               Serial.println(servo_value);
  671.             }
  672.             else if (data_in == '7') {
  673.               Serial.println("Skriv inn Microseconds du vil sende og trykk enter: ");
  674.               String PWM;
  675.               while (int k = 1) {
  676.                 if (Serial.available()) {
  677.                   PWM = Serial.readString();
  678.                   servo_value = PWM.toInt();
  679.                   servo1.writeMicroseconds(servo_value);
  680.                   k = 0;
  681.                   break;
  682.                 }
  683.               }
  684.               Serial.print("Servoverdi er nå : ");
  685.               Serial.println(servo_value);
  686.             }
  687.             else {
  688.               Serial.println("Ikke tillat");
  689.             }
  690.           }
  691.  
  692.         }
  693.  
  694.       }
  695.       Serial.print("Null-pitchvinkelen til servo 1 er satt til: ");
  696.       Serial.println(s1);
  697.  
  698.       servo_value = s2;
  699.       Serial.println("Sett servo2 til null-pitch og skriv OK når du er ferdig med servo 2");
  700.       while (count_motor == 2) {
  701.         if (Serial.available()) {
  702.           data_in = Serial.read();
  703.  
  704.           if (data_in == '0') {
  705.             s2 = servo_value;
  706.             count_motor++;
  707.           }
  708.           else {
  709.             if (data_in == '1') {
  710.               servo_value++;
  711.               servo2.writeMicroseconds(servo_value);
  712.               Serial.print("Servoverdi er nå : " );
  713.               Serial.println(servo_value);
  714.             }
  715.             else if (data_in == '2') {
  716.               servo_value--;
  717.               servo2.writeMicroseconds(servo_value);
  718.               Serial.print("Servoverdi er nå : " );
  719.               Serial.println(servo_value);
  720.             }
  721.  
  722.             else if (data_in == '7') {
  723.               Serial.println("Skriv inn Microseconds du vil sende og trykk enter: ");
  724.               String PWM;
  725.               while (int k = 1) {
  726.                 if (Serial.available()) {
  727.                   PWM = Serial.readString();
  728.                   servo_value = PWM.toInt();
  729.                   servo2.writeMicroseconds(servo_value);
  730.                   k = 0;
  731.                   break;
  732.                 }
  733.               }
  734.               Serial.print("Servoverdi er nå : ");
  735.               Serial.println(servo_value);
  736.             }
  737.  
  738.             else {
  739.               Serial.println("Ikke tillat");
  740.             }
  741.           }
  742.         }
  743.       }
  744.       Serial.print("Null-pitchvinkelen til servo 2 er satt til: ");
  745.       Serial.println(s2);
  746.  
  747.  
  748.       servo_value = s3;
  749.       Serial.println("Sett servo3 til null-pitch og skriv OK når du er ferdig med servo 3");
  750.       while (count_motor == 3) {
  751.         if (Serial.available()) {
  752.           data_in = Serial.read();
  753.  
  754.           if (data_in == '0') {
  755.             s3 = servo_value;
  756.             count_motor++;
  757.           }
  758.           else {
  759.             if (data_in == '1') {
  760.               servo_value++;
  761.               servo3.writeMicroseconds(servo_value);
  762.               Serial.print("Servoverdi er nå : " );
  763.               Serial.println(servo_value);
  764.             }
  765.             else if (data_in == '2') {
  766.               servo_value--;
  767.               servo3.writeMicroseconds(servo_value);
  768.               Serial.print("Servoverdi er nå : " );
  769.               Serial.println(servo_value);
  770.             }
  771.  
  772.             else if (data_in == '7') {
  773.               Serial.println("Skriv inn Microseconds du vil sende og trykk enter: ");
  774.               String PWM;
  775.               while (int k = 1) {
  776.                 if (Serial.available()) {
  777.                   PWM = Serial.readString();
  778.                   servo_value = PWM.toInt();
  779.                   servo3.writeMicroseconds(servo_value);
  780.                   k = 0;
  781.                   break;
  782.                 }
  783.               }
  784.               Serial.print("Servoverdi er nå : ");
  785.               Serial.println(servo_value);
  786.             }
  787.  
  788.             else {
  789.               Serial.println("Ikke tillat");
  790.             }
  791.           }
  792.         }
  793.       }
  794.       Serial.print("Null-pitchvinkelen til servo 3 er satt til: ");
  795.       Serial.println(s3);
  796.  
  797.  
  798.  
  799.       servo_value = s4;
  800.       Serial.println("Sett servo4 til null-pitch og skriv OK naar du er ferdig med servo 4");
  801.       while (count_motor == 4) {
  802.         if (Serial.available()) {
  803.           data_in = Serial.read();
  804.  
  805.           if (data_in == '0') {
  806.             s4 = servo_value;
  807.             count_motor++;
  808.           }
  809.           else {
  810.             if (data_in == '1') {
  811.               servo_value++;
  812.               servo4.writeMicroseconds(servo_value);
  813.               Serial.print("Servoverdi er nå : " );
  814.               Serial.println(servo_value);
  815.             }
  816.             else if (data_in == '2') {
  817.               servo_value--;
  818.               servo4.writeMicroseconds(servo_value);
  819.               Serial.print("Servoverdi er nå : " );
  820.               Serial.println(servo_value);
  821.             }
  822.  
  823.             else if (data_in == '7') {
  824.               Serial.println("Skriv inn Microseconds du vil sende og trykk enter: ");
  825.               String PWM;
  826.               while (int k = 1) {
  827.                 if (Serial.available()) {
  828.                   PWM = Serial.readString();
  829.                   servo_value = PWM.toInt();
  830.                   servo4.writeMicroseconds(servo_value);
  831.                   k = 0;
  832.                   break;
  833.                 }
  834.               }
  835.               Serial.print("Servoverdi er nå : ");
  836.               Serial.println(servo_value);
  837.             }
  838.  
  839.             else {
  840.               Serial.println("Ikke tillat");
  841.             }
  842.           }
  843.         }
  844.       }
  845.       Serial.print("Null-pitchvinkelen til servo 4 er satt til: ");
  846.       Serial.println(s4);
  847.  
  848.     }
  849.  
  850.     Serial.println("Kalibræææring av servoer skal nå være fullført!");
  851.   }
  852.   Serial.println("Done");
  853. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!