'___ ____ _ ____ ____ ___ ___ ____ ____ ' | |__/ | | | | |__] | |___ |__/ ' | | \ | |___ |__| | | |___ | \ ' By William Thielicke(w.th@gmx.de) ' ____________pin Out________________ ' | | ' | Arduino Pin = Mega168 Pin | ' | | ' | Servo - > D6 = Portd.6 | ' | Receiver - > D3 = Portd.3 = Int1 | ' | Led1 - > D7 = Portd.7 | ' | Led2 - > D8 = Portb.0 | ' | Led3 - > D9 = Portb.1 | ' | Sda - > D4 = Portd.4 | ' | Scl - > D5 = Portd.5 | ' | Arduino Led_grn = Portb.5 | ' | Voltage meas = ADC(0) | ' | Gyro Yaw = ADC(1) | ' | Gyro Nick = ADC(2) | ' | Gyro Roll = ADC(3) | ' | X Acc = ADC(4) | ' | Y Acc = ADC(5) | ' |___________________________________| ' Version 1.0 - Shrediquette DLX ' Bremen , Germany , 01.11.2009 ' http://Shrediquette.blogspot.com ' Published under the CreativeCommons Attribution-Noncommercial-Share Alike 3.0 Germany ' http://creativecommons.org/licenses/by-nc-sa/3.0/de/deed.en '-- 'Preface: 'A tricopter is a flying thing that uses three motors + propellers (arranged in a horizontal triangle) to stay airborne. 'The speed of each of these motors can be controlled independently. Additionally, the motor at the rear can be rotated left and right via a servo. 'One propeller turns clockwise, two propellers turn counter-clockwise. 'Gyroscopes (measuring angular velocity) and accelerometers (measuring accelerations) are used to stabilize the inherently unstable 'system. The pilot can control the tricopter with a remote control. Height, nick, roll and yaw are controlled via 2 sticks on the remote control. 'A 3-position switch turns on the motors and selects control mode. Hence a 5-channel transmitter & receiver is needed. 'This specific tricopter uses two different control modes: Firstly, "hover mode" where the pilot controls the angle of the tricopter. Moving 'the sticks 10 degrees tilts the tricopter ~10 degrees. The copter will begin to accelerate in the desired direction. If you release the sticks, 'the copter will automatically level. But it will keep the velocity it had before (if we disregard aerodynamic drag). 'Secondly, "acrobatic mode", where the pilot controls the angular velocity of the copter. Moving the sticks "a bit" will result in slow rotation 'of the tricopter. Moving the sticks further will result in faster rotation. The copter will always keep the angle it had before if you release 'the sticks. Yaw control is always "heading-hold" = "velocity control". 'The source code can be roughly divided in ten parts: '1 (startup) : Initialization of hardware & variables & subs & startup procedure '2 (gyro) : Measurement of angular velocities via gyroscopes, PID control loop '3 (acc) : Measurement of accelerations (mainly gravitation) '4 (mixer) : Combination of sensor and stick inputs '5 (send_mots): Sending out motor signals via I2C '6 (led) : Flashing of some leds '7 (voltage) : Measuring the voltage of the battery '8 (failsave) : Monitoring receiver (RX) signal '9 (receiver) : Reading RX's sum-signal '10 (servo) : Creation of servo PWM signal 'Please Note : If you never flew a radio controlled airplane or heli before, practice on a RC-simulator first! Even if the copter is auto-stable, 'there are always situations where you quickly have to react in the right way. You can only learn that by practicing (or by paying a lot of 'money for spare parts...) 'The obligatory end-note: This is not a toy, although it is a lot of fun to play with it... '===CHIP SETTINGS=== $regfile = "m168def.dat" $framesize = 32 $swstack = 32 $hwstack = 32 $crystal = 16000000 $baud = 38400 '===READ RX SETTINGS=== Config Timer0 = Timer , Prescale = 256 , Capture Edge = Falling On Timer0 Detectrxpause 'timer overflow = pause in receiver's signal Config Int1 = Falling On Int1 Getreceiver 'falling edge -> measure time '===SERVO SETTINGS=== Config Timer1 = Timer , Prescale = 8 Timer1 = 62535 On Timer1 Servoirq 'creates the servo PWM '===SERIAL IN SETTINGS=== Config Serialout = Buffered , Size = 100 '===ADC SETTINGS=== Config Adc = Single , Prescaler = Auto , Reference = Avcc 'avcc reference @ arduino =5V '===PORT SETTINGS=== Config Scl = Portd.5 'for ESCs Config Sda = Portd.4 'for ESCs Config Pind.6 = Output 'servo Portd.6 = 0 'servo Config Pind.7 = Output 'led 1 Config Pinb.0 = Output 'led 2 Config Pinb.1 = Output 'led 3 Config Pinb.5 = Output 'Arduino LED_grn '===DECLARATIONS=== '--Global-- Dim I As Word 'counter var Dim Lf As Single 'stick sensitivity Dim Lfdynamic As Single 'dynamic stick sensitvity (used for flying loopings) Dim Lf_tail As Integer 'yaw stick sensitivity Dim Motors_on As Bit 'self explanatory, right...?! Dim State As Byte 'Contains selected control loop state: 0 = Motors off, 1 = Acrobatic mode, 2 = Hover mode Dim Old_state As Byte 'Contains the state of the last cycle Dim Lookup_pos As Integer '--Servo control-- Dim Servotail As Word 'servo position, neutral = 62535, full left = 61535, full right = 63535 '--Read Receiver (Rx)-- Dim Empf(5) As Word 'data from getreceiver interrupt Dim Sempf(5) As Integer 'rescaled data Dim Aempfh(5) As Integer 'higher limit for rx noise filter Dim Aempfl(5) As Integer 'lower limit for rx noise filter Dim Channel As Byte 'current channel to be read out Dim Rc_on_counter As Word 'counter that counts the amount of correct rx signals '--I2C-- Const M_h = &H52 'i2c address: motor at the back Const M_l = &H54 'i2c address: motor at the left Const M_r = &H56 'i2c address: motor at the right Dim Ms_h_i As Integer 'contains motor nominal value as integer Dim Ms_r_i As Integer 'contains motor nominal value as integer Dim Ms_l_i As Integer 'contains motor nominal value as integer Dim Ms_h As Word 'converts motor nominal value Dim Ms_r As Word 'converts motor nominal value Dim Ms_l As Word 'converts motor nominal value '--Gyros-- Dim Roll_init As Word 'gyro offset Dim Nick_init As Word 'gyro offset Dim Yaw_init As Word 'gyro offset Dim Meas_roll As Integer 'angular velocity reading from gyros Dim Setpoint_roll As Single 'Stick position Dim Error_roll As Single 'Stick position - current position Dim Error_roll_sum As Single 'Integral of the above Dim Meas_angle_roll As Single 'the angle of the copter including acc signal (only in Hover mode) Dim Meas_nick As Integer 'angular velocity reading from gyros Dim Setpoint_nick As Single 'Stick position Dim Error_nick As Single 'Stick position - current position Dim Error_nick_sum As Single 'Integral of the above Dim Meas_angle_nick As Single 'the angle of the copter including acc signal (only in Hover mode) Dim P_set_roll As Single 'P output including scaling (gain) Dim I_set_roll As Single 'I output including scaling (gain) Dim D_set_roll As Single 'D output including scaling (gain) Dim P_set_roll_int As Integer 'converts to integer Dim I_set_roll_int As Integer 'converts to integer Dim D_set_roll_int As Integer 'converts to integer Dim P_set_nick As Single 'P output including scaling (gain) Dim I_set_nick As Single 'I output including scaling (gain) Dim D_set_nick As Single 'D output including scaling (gain) Dim P_set_nick_int As Integer 'converts to integer Dim I_set_nick_int As Integer 'converts to integer Dim D_set_nick_int As Integer 'converts to integer Dim Yaw_gyro As Integer 'angular velocity reading from gyroscope Dim Yaw_gyro_i As Integer 'yaw gyro var Dim Yaw_gyro_scale As Single 'scaled-down values Dim Yaw_gyro_scale_int As Integer 'converts to integer Dim Yaw_gyro_i_scale As Single 'scaled-down values Dim Yaw_gyro_i_scale_int As Integer 'converts to integer Dim P_sens As Single 'roll/nick P control loop sensitivity Dim I_sens As Single 'roll/nick I control loop sensitivity Dim D_sens As Single 'roll/nick D control loop sensitivity Dim Yaw_p_sens As Single 'yaw P control loop sensitivity Dim Yaw_i_sens As Single 'yaw I control loop sensitivity Dim Gyro_i_enable As Bit '0 or 1: don't perform gyro integration when motors off '--Acc-- Dim Xacc As Integer 'accelerometer nick Dim Yacc As Integer 'accelerometer roll Dim Accy_temp As Single 'used for complementary filtering Dim Accx_temp As Single 'used for complementary filtering '--Failsave-- Dim Failure As Byte 'increases when there are errors in receiver reading Dim Meanrx(5) As Word 'used for receiver noise filter '--Mixer-- Dim Vorwahl As Word 'throttle idle up Dim Yaw_diff As Integer 'yaw gyro var Dim Tailgas As Integer 'increases throttle of rear motor if tilted '--Voltage Check-- Dim Spann As Word 'reads voltage from adc Dim Spannmittel As Single 'used for filtering Dim Spann_alt As Word 'used for filtering (alt is german and means "old") Dim Spann_alt2 As Word 'used for filtering Dim Spann_alt3 As Word 'used for filtering Spann_alt = 1023 'prepare the filter Spann_alt2 = 1023 'prepare the filter Spann_alt3 = 1023 'prepare the filter Dim Lowvoltage As Bit 'will be 1 if voltage is low '--LEDs-- Dim Ledcount As Word 'used for bling bling Dim Blinker As Word 'used for low voltage bling bling '--Subs-- Declare Sub Mixer 'prepare all signals for motors and servo Declare Sub Gyro 'read and prepare gyro's Declare Sub Acc 'read and rescale accelerometer Declare Sub Led 'bling bling Declare Sub Send_mots 'check and reshape motor signals and send them via i2c Declare Sub Voltage 'check voltage Declare Sub Failsave 'safety: turn off motors if reception is lost '--Alias's Led_1 Alias Portd.7 Led_2 Alias Portb.0 Led_3 Alias Portb.1 Led_grn Alias Portb.5 'a green led on the arduino mini pro '===START CONDITIONS=== Set Led_1 'light up led's after reset Set Led_2 Set Led_3 '--Variables-- 'prepare some variables Motors_on = 0 State = 0 Ledcount = 1 Servotail = 62535 'neutral position for the servo Gyro_i_enable = 0 '--Startup 2-- Enable Timer0 Enable Timer1 Enable Interrupts Enable Int1 Start Adc I2cinit Ms_h = 0 : I2csend M_h , Ms_h 'turn off the motors Ms_r = 0 : I2csend M_r , Ms_r Ms_l = 0 : I2csend M_l , Ms_l Led_grn = 0 Waitms 500 Reset Led_1 'turn off led's Reset Led_2 Reset Led_3 '===RC turned on...?=== Rc_test: 'switch (channel5) and throttle stick must be zero in order to proceed If Empf(5) > 67 Or Empf(5) < 59 Or Empf(1) > 67 Or Empf(1) < 59 Then 'sticks not in correct position If Rc_on_counter > 0 Then Decr Rc_on_counter End If Else 'sticks in correct position Incr Rc_on_counter End If If Rc_on_counter = 0 Then 'sticks not in correct position Toggle Led_1 'give signal to pilot Toggle Led_3 Waitms 75 End If If Rc_on_counter < 500 Then 'if we didn't get the correct stick position 500 times... Waitms 1 Goto Rc_test '... return and check again End If Wait 1 'If we got the correct stick positions 500 times... proceed Led_grn = 1 Reset Led_1 Reset Led_3 '=== Measure Gyro Offset (don't move copter now!) === Roll_init = 0 Nick_init = 0 Yaw_init = 0 For I = 1 To 100 'read and sum up gyro's 100 times Roll_init = Roll_init + Getadc(3) Nick_init = Nick_init + Getadc(2) Yaw_init = Yaw_init + Getadc(1) Spann = Getadc(0) Toggle Led_grn If I < 33 Then 'bling bling Toggle Led_1 End If If I > 33 And I < 66 Then Reset Led_1 Toggle Led_2 End If If I > 66then Reset Led_2 Toggle Led_3 End If Waitms 13 Next Reset Led_1 Reset Led_2 Reset Led_3 Roll_init = Roll_init / 100 'get the average of the last 100 readings Nick_init = Nick_init / 100 Yaw_init = Yaw_init / 100 'Prepare receiver noise filter... Meanrx(1) = 63 Meanrx(2) = 100 Meanrx(3) = 100 Meanrx(4) = 100 Meanrx(5) = 63 '==============MAIN LOOP================== 'All this does is calling subs in a loop all the time Do Gyro Acc Mixer Send_mots Led Voltage Failsave Loop End '===GYRO=== Sub Gyro If Old_state <> State Then 'compare state of last cycle with current state Error_roll = 0 'otherwise things would start mixing up... Error_nick = 0 Meas_roll = 0 Meas_nick = 0 Error_roll_sum = 0 Error_nick_sum = 0 Meas_angle_roll = 0 Meas_angle_nick = 0 Yaw_gyro_i = 0 Yaw_diff = 0 End If Old_state = State If State = 1 Then 'ACROBATIC MODE = Angular velocity control '--Roll-- Meas_roll = Getadc(3) 'get gyro signal Meas_roll = Roll_init - Meas_roll 'subtract offset Setpoint_roll = Sempf(2) * Lf 'sempf(2) is roll stick position Error_roll = Meas_roll - Setpoint_roll 'calculate difference between angular velocity and stick position Error_roll_sum = Error_roll_sum + Error_roll 'integrate the above P_set_roll = Error_roll * P_sens 'multiply with gain If Gyro_i_enable = 0 Then 'don't integrate when motors off Error_roll_sum = 0 End If I_set_roll = Error_roll_sum * I_sens 'multiply with gain D_set_roll = 0 'there is no D in Acro mode... D only yields noise '--Nick-- Meas_nick = Getadc(2) 'see above Meas_nick = Meas_nick - Nick_init Setpoint_nick = Sempf(3) * Lfdynamic Error_nick = Meas_nick - Setpoint_nick Error_nick_sum = Error_nick_sum + Error_nick P_set_nick = Error_nick * P_sens If Gyro_i_enable = 0 Then Error_nick_sum = 0 End If I_set_nick = Error_nick_sum * I_sens D_set_nick = 0 End If If State = 2 Then 'HOVER MODE = Angle control '--Roll-- Meas_roll = Getadc(3) 'get gyro signal Meas_roll = Roll_init - Meas_roll 'subtract offset Meas_angle_roll = Meas_angle_roll + Meas_roll 'integrate gyro signal 'this might require some further explanation: 'The gyroscopes can only measure differences in rotational speed. Integrated over a long time (e.g. 11 minutes of flight) 'the angle calculated from gyro's alone loses precision. Here, the acc comes into play: It always knows the angle of the 'copter, but it reacts pretty slowly. And it contains quite some noise. The following lines of code combine the fast 'signal of the gyroscopes and the absolute precision of the accelerometer. In the end, you get the best out of both worlds: Meas_angle_roll = Meas_angle_roll * 0.99 'take 0.99 of gyro integral and 0.01 of acc... Accy_temp = Yacc * 0.01 Meas_angle_roll = Meas_angle_roll - Accy_temp '...and put these two together (complementary filtering) Setpoint_roll = Sempf(2) * Lf 'roll stick position * stick sensitivity Error_roll = Meas_angle_roll - Setpoint_roll 'current angle minus desired angle (stick position) Error_roll_sum = Error_roll_sum + Error_roll 'integral of an integral P_set_roll = Error_roll * P_sens 'multiply with gain If Gyro_i_enable = 0 Then Error_roll_sum = 0 End If I_set_roll = Error_roll_sum * I_sens 'multiply with gain D_set_roll = Meas_roll * D_sens 'multiply with gain '--Nick-- Meas_nick = Getadc(2) 'see above Meas_nick = Meas_nick - Nick_init Meas_angle_nick = Meas_angle_nick + Meas_nick Meas_angle_nick = Meas_angle_nick * 0.99 Accx_temp = Xacc * 0.01 Meas_angle_nick = Meas_angle_nick - Accx_temp Setpoint_nick = Sempf(3) * Lf Error_nick = Meas_angle_nick - Setpoint_nick Error_nick_sum = Error_nick_sum + Error_nick P_set_nick = Error_nick * P_sens If Gyro_i_enable = 0 Then Error_nick_sum = 0 End If I_set_nick = Error_nick_sum * I_sens D_set_nick = Meas_nick * D_sens End If 'convert rescaled values (single) to integers. P_set_roll_int = P_set_roll I_set_roll_int = I_set_roll D_set_roll_int = D_set_roll P_set_nick_int = P_set_nick I_set_nick_int = I_set_nick D_set_nick_int = D_set_nick '--Yaw-- Yaw_gyro = Getadc(1) 'get yaw rate from gyro Yaw_gyro = Yaw_gyro - Yaw_init 'subtract offset Yaw_diff = Sempf(4) * Lf_tail 'yaw stick position Yaw_diff = Yaw_diff - Yaw_gyro 'stick position - current angular velocity Yaw_gyro_i = Yaw_gyro_i + Yaw_diff 'integral of the above If Yaw_gyro_i > 32000 Then 'protect from overflow Yaw_gyro_i = 32000 End If If Yaw_gyro_i < -32000 Then 'protect from overflow Yaw_gyro_i = -32000 End If Yaw_gyro_scale = Yaw_diff * Yaw_p_sens 'multiply with gain If Gyro_i_enable = 0 Then 'integrate only when motors on Yaw_gyro_i = 0 End If Yaw_gyro_i_scale = Yaw_gyro_i * Yaw_i_sens 'multiply with gain Yaw_gyro_scale_int = Yaw_gyro_scale 'convert single to integer Yaw_gyro_i_scale_int = Yaw_gyro_i_scale 'convert single to integer End Sub '===ACCELEROMETER=== Sub Acc If State <> 1 Then 'only used in HOVER MODE and when motors off Xacc = Getadc(4) 'read nick "angle" Yacc = Getadc(5) 'read roll "angle" Xacc = 515 - Xacc 'these values are the ACC offsets. They were determined in flight (hovering in place) Yacc = 524 - Yacc Yacc = Yacc * 120 'make the amplitude if gyro_integrals and acc similar Xacc = Xacc * 120 ' (necessary for successful complementary filtering) End If End Sub '===MIXER=== Sub Mixer '--Noise filter-- For I = 1 To 5 Meanrx(i) = Meanrx(i) * 3 Meanrx(i) = Meanrx(i) + Empf(i) '"lowpass filter" of the RC signal Shift Meanrx(i) , Right , 2 ' (=divide by 4) Aempfh(i) = Meanrx(i) + 17 'upper acceptable fluctuation Aempfl(i) = Meanrx(i) - 17 'lower acceptable fluctuation If Empf(i) > Aempfh(i) Or Empf(i) < Aempfl(i) Then 'compare allowed fluctuation with current rc reading Empf(i) = Meanrx(i) 'if fluctuation was too high -> replace with averaged value 'Print "Error reading channel: " ; I 'handy to check for RC problems End If Next 'Empf(1-5) are filled in "getreceiver". They usually contain values ranging from 63 - 137. 'Empf(1) is the throttle stick. it will be rescaled differently. If Empf(1) > 61 And Empf(1) < 139 Then 'don't process values that can't be correct Sempf(1) = Empf(1) - 61 Sempf(1) = Sempf(1) * 3 '==> values ranging from 3 (stick at bottom) to 228 (full throttle) End If 'Empf(2-5) are nick, roll, yaw and idle up switch For I = 2 To 5 If Empf(i) > 61 And Empf(i) < 139 Then 'don't process values that can't be correct Sempf(i) = Empf(i) - 100 'convert to values ranging from -37 to +37 End If Next 'Dynamic LF: Makes the copter react non-linearly to nick. I use this for flying loopings. The more I pull, the faster(exponential) the copter will nick If State = 1 Then 'only when in acro mode Lookup_pos = Abs(sempf(3)) 'make a variable that grows when stick is out of centre Lookup_pos = Lookup_pos - 25 If Lookup_pos < 0 Then Lookup_pos = 0 End If If Lookup_pos >= 13 Then Lookup_pos = 13 End If If Lookup_pos <> 0 Then Lfdynamic = Lookup(lookup_pos , Dta) 'look for a new sensitivity factor in a table Else Lfdynamic = Lf End If End If 'ACRO MODE (angular velocity control, ACC = off) If Sempf(5) > -3 And Sempf(5) < 3 Then 'switch in middle = motors on; sempf(5) is the idle up switch Vorwahl = 20 'minimum throttle Gyro_i_enable = 1 'start integrating the gyroscope signals Lf = 5.8 'nick and roll sensitivity Lf_tail = 9 'yaw sensitivity P_sens = 0.45 'Gyro_P sensitivity roll and nick I_sens = 0.001 'Gyro_I Sensitivity roll and nick D_sens = 0 'no D available in Acro mode (calculating it only yields noise) Yaw_p_sens = 8 'Gyro_P Sensitivity yaw Yaw_i_sens = 0.05 'Gyro_I Sensitivity yaw Motors_on = 1 State = 1 'flight mode: acrobatic End If 'HOVER MODE (angle control, ACC = on) If Sempf(5) >= 33 And Sempf(5) < 39 Then 'switch top = motors on Vorwahl = 20 'minimum throttle Gyro_i_enable = 1 'start integrating the gyroscope signals Lf = 450 'nick and roll sensitivity (much bigger, because a different control loop is used) Lf_tail = 8 'yaw sensitivity P_sens = 0.0025 'Gyro_P sensitivity roll and nick (corresponds to Gyro_I in acro mode) I_sens = 0.0000015 'Gyro_I sensitivity roll and nick (this is a double integral, thats why the value is so low) D_sens = 0.4 'Gyro_D sensitivity roll and nick (corresponds to Gyro_P in acro mode) Yaw_p_sens = 6 'Gyro_P Sensitivity yaw Yaw_i_sens = 0.04 'Gyro_I Sensitivity yaw Motors_on = 1 State = 2 'flight mode: hover End If If Sempf(5) <= -33 And Sempf(5) > -39 Then 'switch bottom = motors off If Sempf(1) < 10 Then 'only turn motors off when throttle stick is also on bottom Vorwahl = 0 'no minimum throttle = motors off Gyro_i_enable = 0 'do not integrate gyros anymore Motors_on = 0 State = 0 'flight mode: off Lf = 0 'don't react to stick movements Lf_tail = 0 P_sens = 0 'don't react to gyros I_sens = 0 'don't react to gyros D_sens = 0 'don't react to gyros Yaw_p_sens = 0 'don't react to gyros Yaw_i_sens = 0 'don't react to gyros End If End If '-Mix components- If Motors_on = 1 Then 'only when motors are running '-MOTOR 1- (front left) Ms_l_i = Sempf(1) + Vorwahl 'throttle stick (3-228) + Vorwahl(20) Ms_l_i = Ms_l_i + P_set_roll_int 'add the gyro measurements (note the differences in "+" and "-") Ms_l_i = Ms_l_i + P_set_nick_int Ms_l_i = Ms_l_i + I_set_roll_int Ms_l_i = Ms_l_i + I_set_nick_int Ms_l_i = Ms_l_i + D_set_roll_int Ms_l_i = Ms_l_i + D_set_nick_int '-MOTOR 2- (front right) Ms_r_i = Sempf(1) + Vorwahl Ms_r_i = Ms_r_i - P_set_roll_int Ms_r_i = Ms_r_i + P_set_nick_int Ms_r_i = Ms_r_i - I_set_roll_int Ms_r_i = Ms_r_i + I_set_nick_int Ms_r_i = Ms_r_i - D_set_roll_int Ms_r_i = Ms_r_i + D_set_nick_int '-MOTOR 3- (back) Ms_h_i = Sempf(1) + Vorwahl Ms_h_i = Ms_h_i - P_set_nick_int Ms_h_i = Ms_h_i - I_set_nick_int Ms_h_i = Ms_h_i - D_set_nick_int 'when yawing very fast, throttle for the motor at the back is increased Tailgas = Yaw_gyro_scale_int + Yaw_gyro_i_scale_int 'if these values are big, the motor at the back was tilted Tailgas = Abs(tailgas) / 100 Ms_h_i = Ms_h_i + Tailgas Else 'motors off Ms_l_i = 0 Ms_r_i = 0 Ms_h_i = 0 End If '-Servo control- 'Servo neutral= 62535. First I add 32535, then I again add 30000 (=62535). This makes sense because I wan't 'to protect "Servotail" from overflow. Servotail = 32535 - Yaw_gyro_scale_int 'add gyro_P Servotail = Servotail - Yaw_gyro_i_scale_int 'add gyro_I If Servotail > 33534 Then 'Overflow Protection Servotail = 33534 End If If Servotail < 31536 Then 'Overflow Protection Servotail = 31536 End If Servotail = Servotail + 30000 'add the remaining 30000 for neutral position End Sub '===MOTORS I2C=== Sub Send_mots If Ms_l_i > 255 Then 'cap motor signals (upper limit = 255) Ms_l_i = 255 End If If Motors_on = 1 Then If Ms_l_i < Vorwahl Then 'cap motor signals (lower limit = vorwahl) Ms_l_i = Vorwahl End If End If If Ms_l_i < 0 Then 'cap motor signals (lower limit = 0) Ms_l_i = 0 End If If Ms_r_i > 255 Then 'cap motor signals (upper limit = 255) Ms_r_i = 255 End If If Motors_on = 1 Then If Ms_r_i < Vorwahl Then 'cap motor signals (lower limit = vorwahl) Ms_r_i = Vorwahl End If End If If Ms_r_i < 0 Then 'cap motor signals (lower limit = 0) Ms_r_i = 0 End If If Ms_h_i > 255 Then 'cap motor signals (upper limit = 255) Ms_h_i = 255 End If If Motors_on = 1 Then If Ms_h_i < Vorwahl Then 'cap motor signals (lower limit = vorwahl) Ms_h_i = Vorwahl End If End If If Ms_h_i < 0 Then 'cap motor signals (lower limit = 0) Ms_h_i = 0 End If Ms_l = Ms_l_i 'convert integer to word Ms_h = Ms_h_i Ms_r = Ms_r_i If Failure < 15 Then 'if there are NO problems with the receiver: run motors I2csend M_h , Ms_h I2csend M_r , Ms_r I2csend M_l , Ms_l Else 'if there are problems with the receiver: turn off motors I2csend M_h , 0 I2csend M_r , 0 I2csend M_l , 0 End If End Sub '===LEDs=== Sub Led If Ledcount < 500 Then Incr Ledcount If Lowvoltage = 0 Then If Ledcount = 130 Or Ledcount = 140 Or Ledcount = 230 Or Ledcount = 240 Then 'flash LEDs Toggle Led_1 End If If Ledcount = 100 Or Ledcount = 110 Or Ledcount = 200 Or Ledcount = 210 Then Toggle Led_2 End If If Ledcount = 160 Or Ledcount = 170 Or Ledcount = 260 Or Ledcount = 270 Then Toggle Led_3 End If If Ledcount = 470 Then Toggle Led_grn End If End If If Lowvoltage = 1 Then 'when battery is low, blink the LEDs in an annoying manner If Blinker < 30 Then Incr Blinker Else Blinker = 1 Toggle Led_1 Toggle Led_2 Toggle Led_3 End If Reset Led_grn End If Else 'ledcount> 500 Toggle Led_grn 'toggle green led every 500 cycles (I use this to determine the speed of the program) Ledcount = 1 End If End Sub '===VOLTAGE=== Sub Voltage If Ledcount = 500 Then 'we'll only check voltage every 500 cycles (==> about 1 Hz) Spann = Getadc(0) Spannmittel = Spann + Spann_alt 'very unprofessional moving average filter Spannmittel = Spannmittel + Spann_alt2 Spannmittel = Spannmittel + Spann_alt3 Spannmittel = Spannmittel / 4 Spann_alt = Spann Spann_alt2 = Spann_alt Spann_alt3 = Spann_alt2 'ADC = 80.775 * voltage - 4.783 'experimentally determinded scale factor of the ADC ' 10,50 V -> 843 ' 10,40 V -> 835 ' 10,30 V -> 827 ' 10,20 V -> 819 ' 10,10 V -> 811 ' 10,00 V -> 803 ' 9,90 V -> 795 ' 9,80 V -> 787 ' 9,70 V -> 779 ' 9,60 V -> 771 ' 9,50 V -> 763 ' 9,40 V -> 755 ' 9,30 V -> 746 ' 9,20 V -> 738 If Spannmittel < 795 Then '=> 9.9V Lowvoltage = 1 'set low-voltage bit when voltage is low End If End If End Sub '===FAILSAVE=== Sub Failsave If Channel >= 11 Then '"channel" will be greater than 11 if something with the receiver went wrong If Failure < 255 Then Incr Failure 'If this happens: increase Failure End If End If If Channel < 11 Then '"Channel" will be lower than 11 if rx experiences NO problems If Failure > 0 Then Decr Failure 'Failure will accordingly be decreased End If End If End Sub '===SERVO CONTROL=== Servoirq: 'generate servo PWM pulse If Portd.6 = 0 Then Timer1 = Servotail Portd.6 = 1 Else Portd.6 = 0 Timer1 = 35535 'after a pulse, pause for 15ms (= 66Hz instead of 50Hz, the latter is typically used for servos) End If 'but 66Hz gives a slightly faster and more powerful servo reaction Return '===READ RX=== Getreceiver: 'falling edge detection If Channel > 0 And Channel < 6 Then 'fill empf(1-5) Empf(channel) = Timer0 End If Timer0 = 6 'preload for 4.096 ms Incr Channel 'if no falling edge was detected for a longer period, channel will increase above 11 Return 'that means that there are problems with the receiver Detectrxpause: Channel = 0 Return Dta: 'table lookup for dynamic lf (increasing maneuvrability when stick is out of the centre) Data 5.8! , 6.1! , 6.3! , 6.5! , 6.8! , 7.2! , 7.6! , 8.1! , 8.7! , 9.4! , 10.2! , 11! , 12! , 13.6!