'**************************************************************** '* Name : Bug_Hub.BAS * '* Author : Godfried-Willem RAES * '* Notice : Copyleft (c) 2020 Logosoft Public Domain * '* Date : 11-12-2016 * '* Version : 1.1 * '* Notes : First working version 28.12.2016 * '* Last update: 09.12.2020 * '**************************************************************** '26.08.2012: PIC: 18F2525 On MidiHub board, motor, Tilt_sensor, lites ' Analog tilt Tilt_sensor design with Penny + Giles device. ' For -60 degree to + 60 degree we should read values between 102 and 922 from the ADC ' For -45 degree to + 45 degree we should read values between 205 and 819 from the ADC ' reduced to 7 bits, with 64 as center value we get 25 and 102 as extremes for 45 degrees '--------------------------------------------------------------------------------------------------- '10.12.2016: starting for this code from code for korn, adapting to ' port mappings redefined. ' 3-channel sampler in the low irq rewritten. '11.12.2016: Further coding. ' Implementing sorted timers using TMR0 ' It would be a lot easier to use sword variables for the positions... ' Trying to implement Analog In without the include. Done in Chi also. ' For the vertical movement we can disable the motor in either of the ' extreme positions. '12.12.2016: dur lookups added. '13.12.2016: The easiest way to implement a delay between valves and sound generation would be ' to delay the sound generator midi output a bit. We need to use internally a different ' channel for the sound board in that case. '19.12.2016: Vertical motor needs dir inversion. ' With the stepper controller set to 1/4 step mode, vertical now works. '20.12.2016: Hor motor not yet working... ' On startup the motors do not work... '21.12.2016: We need controllers to block movement. ' Horizontal sensor signal measurement: at rest, 0.409V (in 10 bit: 84) ' activated (touching): 1.5V (in 10 bit: 307) ' Note: left and right are defined as seen from the robot's point of view! ' Horizontal motor sensors mounted. '24.12.2016: Vertical motor changed to work in 1/8 th step mode. '25.12.2016: Seems we do not even use the Velflags variable... ' Further work on horizontal motor coding. ' L and R sensors do not work as they should... ' Got this working now. ' Motor dir does not change... hardware bug found on the PCB. ' There is a quite unexplainable interference between the two motors... ' commands for the horizontal movement cause the vertical one to move... ' Hardware changed: all mosfets on the ctrl.lines for the gecko drive removed ' Thus all logic now becomes positive. Of course we could also have used P-channel ' mosfets here, sourcing current. '26.12.2016: New testing session ' Gecko direction signal seems not working. (Port C.4, RC4) ' Strange interference between motors remains... ' Measured: the units in Timvals[] are 25us, to the clockpulse ' for the horizontal motor is 100us long. ' The step frequency varies between 465Hz and 1200Hz, so this ' is a lot faster than the sampling rate... ' The direction bits on PIC level work the way they should '27.12.2016: Calibration procedure added. Thus runs only on cold boot. ' Got this one working, but we still have glitches on ' vertical motor everytime we process incoming midi... ' The behavior of the horizontal direction bit remains erratic. ' Interference seems to have stopped now with the new sampling settings. ' Measured params for the calibration procedure: clock pulses are 100us ' Repetition frequency is 470 Hz (steprate) or period 2.12ms. '28.12.2016: as we suspect something wrong with the Gecko controller, we replaced it ' with a spare one. ' Doing this made no difference... ' Replacing the PIC didn't help neither. ' Alternative strategy: sample the R-L sensors in task 4 only and use the ' Timer3 IRQ sampler only for AN0. ' Now it seems to work in principle... ' 2 220pF added over the endsensor inputs against glitches and some more ' bypass cap's added on the board. ' However, we loose steps on horizontal movements. ' Acceleration and deceleration added on horizontal motor movements. ' Movements are quite reliable now. ' This be version 1.0 '09.12.2020: code needs revision as we cannot compile due to compiler upgrades... ' newly added controllers for implemented. ' Compiles o.k. again. - to be checked on now. ' IRQ include needed most upgrading! ' time is now an alias for Cnt Include "18F2525.inc" 'version for the Bug board. (40MHz) Clear ' Mapping defines for midi-events on pin outputs and inputs: ' vertical motor with tilt Tilt_sensor: [Advantec drive, Nanotech motor] $define V_Motor_Enable PORTC.0 ' 0=enable, 1 = OFF, disable motor - inverted by mosfet! $define V_Motor_Clock PORTC.1 ' motor stepping clock, steps on falling edge. At rest 0. $define V_Motor_Dir PORTC.2 ' motor cw/ccw direction of rotation, 0 = CCW, 1 = CW $define Tilt_sensor PORTA.0 ' input port - Penny& Giles tilt Tilt_sensor $define BlueLed PORTB.2 ' position reached indicator. ' horizontal motor with two end sensors [Geckodrive, MAE motor] $define H_Motor_Enable PORTC.5 ' 0=enable, 1 = OFF on the drive. [orange/black wire] $define H_Motor_Clock PORTC.3 ' motor stepping clock, steps on rising edge. At rest 0. [yellow/black wire] $define H_Motor_Dir PORTC.4 ' motor cw/ccw direction of rotation, 0 = CW, 1 = CCW [grey/blue wire] $define L_sensor PORTA.1 ' 0 at rest, becomes 1 if sensor is activated $define R_sensor PORTA.2 ' 0 at rest. $define Red PORTA.4 ' 1W bright smd LED on board $define notused PORTA.5 ' has IRL640 mosfet $define loopcnt PORTA.3 ' for loopspeed measurement only ' following not connected: $define GreenLed PORTB.0 'for code monitoring and debug - movement to the left (backwards) CW $define YellowLed PORTB.1 'for code monitoring and debug - movement to the right (forwards) CCW 'red LED for debug: $define Debug_Led PORTB.5 ' for testing - red led - watchdog ' configure the input and output pins: Clear SSPCON1.5 'RC3 must be available for I/O TRISA = %01000111 'bits set to 0 are output, 1 = input TRISB = %11100000 TRISC = %11000000 'RC1 en RC2 zijn pwm outputs and must be set to output 'RC6 en RC7 zijn USART I/O and must be set to input 'constant definitions: 'initialisations for the midi input parser: Symbol Midichannel = 12 ' Bug_Channel Symbol NoteOff_Status = 128 + Midichannel ' 2 bytes follow Symbol NoteOn_Status = 144 + Midichannel Symbol Keypres_Status = 160 + Midichannel ' 2 bytes follow Symbol Control_Status = 176 + Midichannel Symbol ProgChange_Status = 192 + Midichannel ' 1 byte message Symbol Aftertouch_Status = 208 + Midichannel ' 1 byte follows Symbol Pitchbend_Status = 224 + Midichannel ' lsb msb follow 'application specific constants Symbol NrTasks = 5 ' maximum 16 Symbol V_Hysteresis = 10 ' tilt motor Symbol V_HalfHysteresis = 5 ' tilt motor Symbol V_Period_Default = 679 ' default value for CC33 vertical motor speed Symbol EqPos = 512 ' 10 bit equilibrium position for tilt sensor Symbol MaxDown = 352 ' 102 = 0.5 V on sensor - fully downwards (60 degrees) Symbol MaxUp = 600 ' 922 = 4.5 V on sensor - fully upwards (60 degrees) ' Note: these are the limits of the sensor, the actual ' limits for the movement are a lot more restricted. Symbol H_AccelMax = 100 ' default acceleration range for horizontal motor startup Symbol H_CenterPos = 32768 ' set to center on startup Symbol H_Hysteresis = 8 ' number of motor steps to get back from sensors ' Setup the USART Declare Hserial_Baud = 31250 ' Set baud rate for the USART to MIDI specs. Declare Hserial_TXSTA = 0x24 ' instead of the normal 0x20 - ?? 0x24 'Declare All_Digital = True ' not the case here as we need 3 analog channels ' Declare Hserial_Clear = On ' should clear on errors. Bytes get lost of course... ' Create variables Dim Cnt As Dword System Dim CntHw As Cnt.Word1 'used in the timer0 interrupt, to create a 32 bit timer Dim CntLw As TMR0L.Word 'this is the trick to read both TMR0L and TMR0H 'it makes Cntlw the low word of cnt 'We still have to copy the contents of Lw to Cnt Dim time As Cnt ' was : Dword 'frozen copy of Cnt Dim MaxTim As time.30 ' overflow flag used to reset timer Dim Tim3 As TMR3L.Word ' 16 bit counter for sampler Dim Bytein As Byte System ' midi byte read from buffer Dim StBit As Bytein.7 ' highest bit of ByteIn Dim i As Byte ' general purpose counter ' midi variables Dim statusbyte As Byte System Dim noteUit As Byte System ' note off + release value Dim release As Byte System Dim noteAan As Byte System ' note on + release value Dim velo As Byte System Dim notePres As Byte System ' note pressure + pressure value Dim pres As Byte System Dim Ctrl As Byte System ' continuous controller + value Dim value As Byte System Dim prog As Byte System ' program change + program-byte Dim aft As Byte System ' channel aftertouch Dim pblsb As Byte System ' pitch bend lsb Dim pbmsb As Byte System ' pitch bend msb Dim VelFlags As Word System ' bits 0 - 15 used as flags for active timers Dim VelFlags0 As VelFlags.Byte0 ' alias for bits 0-7 ' Dim VelFlags1 As VelFlags.Byte1 ' bits 8-15 - not used in this code. Dim idx As Byte System ' timer indexes Dim Nxt As Dword ' first timer to time-out Dim Timvals[NrTasks] As Dword ' timer values for the tasks and events 0 - 4 Dim Resort As Byte Dim Resort_flag As Resort.0 ' flag to signal the requirement to resort timers ' Dim CC33 As Byte ' V`motor stepping speed Dim CC10 As Byte ' H motor soll-position Dim CC11 As Byte ' V motor soll-position - midi value 64 = equilibrium Dim CC66 As Byte ' global on/off switch Dim PowerOn As CC66.0 Dim st As Byte System Dim b1 As Byte System Dim b2 As Byte System ' channel counter for sampling: ' Dim Scnt As Byte ' counts 0,1,2 in the low irq ' variables for the vertical tilt sensor: Dim V_SensorVal As Word ' holds the 10bit value read from ADC0 ' has to be word, as we integrate in the low interrupt ' to avoid data copying we could also do: ' Dim V_sensorval as ADRESL.word ' Dim V_RestVal As Word ' Tilt_sensor value read on standstill - 10 bit Dim V_EqPos As Byte ' 8-bit version of shifted standstill value (Eqpos) Dim V_Newval As Word ' used for integration in the low IRQ 10 bit Dim V_NowVal As Byte ' 8 bit and shifted value of above. Calculated in the IRQ Dim V_Sollpos As Byte ' 8 bit = CC11 * 2 \ Dim V_Period As Word ' motor speed, set with ctrl 33 ' on init set to V_period_default = ' variables for the horizontal sensors Dim L_sensorVal As Word ' P&F sensors Horizontal Dim R_sensorVal As Word ' 10 bit values Dim H_sens As Byte Dim L_sens As H_sens.0 ' switch bit: 1 if sensor is touched Dim R_sens As H_sens.1 Dim H_Sollpos As Word ' based on the stepcounter for the motor ' variables for the horizontal movement Dim H_acc As Byte ' for accelleration on motor starts. Dim H_MotPos As Word ' actual position of the motor Dim H_MinPos As Word ' extreme left position Dim H_MaxPos As Word ' extreme right position ' Dim H_CenterPos As Word ' central position, in principle = 32768 Dim H_Traj As Word ' traject in nr. of steps. Dim H_MotorPeriod As Word ' H-motor speed, CC31 Dim H_speed As Byte ' speed during calibration only Dim H_CalibFlag As Byte Dim L_Calib As H_CalibFlag.0 Dim R_Calib As H_CalibFlag.1 Dim CC32 As Byte ' H-motor acceleration controller Dim H_Remweg As Word Dim H_Slowdown As Word Dim H_xtraTim As Word Dim H_dw As Dword ' dword var for calculations Dim H_Dir As Byte Dim H_Left As H_Dir.0 ' if set we are turning left Dim H_Right As H_Dir.1 ' if set we are turning right ' lookup tables Dim Dur[128] As Word Dim Dur5[128] As Word '----------------------------------------------------------------------------------------- ' Load the USART Interrupt handler And buffer read subroutines into memory Dim Ringbuffer[256] As Byte Include "Bug_Hub_Irq.inc" ' our own version for UART And Timer0/3 Interrupt 'make sure we initialize those pins on start up: Low V_Motor_Enable ' motor must be on to go to equilibrium. Low V_Motor_Clock ' steps on the negative going edge, so at rest must be high. With inversion, it's low. Low V_Motor_Dir ' ccw High H_Motor_Enable ' Gecko is disabled with a logic high. Low H_Motor_Clock ' Gecko steps on the rising edge, Low H_Motor_Dir ' cw - high = set to CCW, rotate right. Set H_Left Clear H_Right Low Debug_Led Low GreenLed Low YellowLed Low BlueLed Low Red Low loopcnt Clear CC66 CC11 = 64 ' thus on cold boot, the instrument is assumed to move to equilibrium position V_Sollpos = 128 ' CC11 * 2 Clear H_CalibFlag Clear H_sens ' init sensors to no contact. H_MotorPeriod = 80 ' default speed CC31 H_acc = 100 ' default acceleration CC32 CC32 = 100 '----------------------------------------------------------------------------------------- MAIN: High Debug_Led DelayMS 50 ' wait for stability Low Debug_Led Clear VelFlags0 Set idx Set Timvals ' configure the ADC system: ' coding: 28.12.2016 ADCON1 = %00001100 ' 3 channels, 0,1 and 2 Declare Adin_Res 10 'Declare Adin_Tad 64_FOSC Declare Adin_Stime 100 ' data acquisition time in us Set ADCON2.7 ' right justified - this seems not to be the default! V_SensorVal = ADIn 0 DelayUS 1 L_sensorVal = ADIn 1 DelayUS 1 R_sensorVal = ADIn 2 DelayUS 1 ' at this point we could calibrate horizontal... ' the sampler using TMR3 is not yet active: ' the robot will only start listening to midi, after calibration. GoSub H_Calibrate Init_Usart_Interrupt ' Initiate the USART serial buffer interrupt ' this procedure is in the include file Clear_Serial_Buffer ' Clear the serial buffer and reset its pointers ' in the include as well ' Configure Timer0 for: ' Clear TMR0L and TMR0H registers ' Interrupt on Timer0 overflow ' 16-bit operation ' Internal clock source 40MHz ' 1:256 Prescaler : thus 40MHz / 256 = 156.250kHz ' Opentimer0 (Timer_INT_On & T0_16BIT & T0_SOURCE_INT & T0_PS_1_256) in macro file. Clear T1CON Clear IntConBits_T0IF ' clear interrupt flag Set INTCONBITS_T0IE ' enable interrupt on overflow T0CON = %10000111 ' bit 7 = enable/disable ' bit 6 = 1=8 bit, 0=16 bit ' bit 5 = 1 pin input, 0= Internal Clk0 ' bit 4 = HL or LH transition when bit5 =1 ' bit 3 = 1= bypass prescaler, 0= input from prescaler ' bit 2-0 = prescaler select: 111= 1:256 ' Setup the High priorities for the interrupts ' open and start timer3 for further sampling in the IRQ_service routine: Clear T3CON Clear PIR2BITS_TMR3IF ' clear IRQ flag Set PIE2BITS_TMR3IE ' irq on Clear Tim3 ' Clear TMR3L And TMR3H registers Set RCONbits_IPEN ' Enable priority interrupts Clear IPR2bits_TMR3IP ' Set Timer3 as a low priority interrupt source ' we can also set T3Con in one instruction as: T3CON = %10110001 ' oef, now it works... ' bit 7 = 16 bit mode ' bit 6,3 = 0, 0 ' bit 5,4 = 1:8 prescale ' bit 2 = 0 ' bit 1 = 0 Internal clock = Fosc/4 ' bit 0 : 1= enable timer 3, 0= disable ' maximum count = 52.42ms, 1 tick =0.8uS, lowest freq.=19Hz GoSub Dur_Lookup ' following should cause the vertical motor to adjust. V_EqPos = EqPos >> 2 ' reduce to 8 bits V_Sollpos = 128 ' 8 bits CC11 = 64 ' 7 bit midi ' we need to initialize motor speed also: V_Period = V_Period_Default ' but we have to start a timer, otherwize nothing will happen.. ' power-on will enable the motor! ' for the horizontal motor, we assume it to be in a mid position on cold boot after calibration ' start the main program loop: Do Cnt.Word0 = CntLw ' read timer 0 ' was : time = Cnt , make copy (dword!) but now time is an alias for Cnt ' Create an infinite loop GetMidiIn () ' in Bug_Hub_Irq.inc Bytein = MidiIn ' Read data from the serial buffer, with no timeout ' Start the midi parser. Midi_Parse: If Bytein > Pitchbend_Status Then ' here higher statusses are not implemented. If Bytein > 253 Then '254 = midiclock, 255= reset 'midiclock can interrupt all other msg's... '255 had to be intercepted since thats what we 'get when no new byte flows in (?) GoTo Check_Motors 'Check_Timers 'throw away... Else Clear statusbyte 'reset the status byte GoTo Check_Motors 'Check_Timers 'throw away End If EndIf If StBit =1 Then 'should be faster than If Bytein > 127 Then 'status byte received, bit 7 is set Clear statusbyte 'if on another channel, the statusbyte needs a reset Select Bytein 'eqv to Select case ByteIn Case NoteOff_Status statusbyte = Bytein Set noteUit 'reset value. Cannot be 0 !!! Set release '0 is a valid midi note! Case NoteOn_Status statusbyte = Bytein Set noteAan Set velo Case Keypres_Status ' used for lights statusbyte = Bytein Set notePres Set pres Case Control_Status ' this is the main thing to listen to.... statusbyte = Bytein Set Ctrl Set value Case ProgChange_Status statusbyte = Bytein Set prog Case Aftertouch_Status statusbyte = Bytein Set aft Case Pitchbend_Status statusbyte = Bytein Set pblsb Set pbmsb EndSelect Else 'midi byte is 7 bits Select statusbyte Case 0 'not a message for this channel GoTo Check_Motors 'disregard Case NoteOff_Status If noteUit = 255 Then noteUit = Bytein Else release = Bytein 'message complete, so we can do the action... Select noteUit Case 52 To 94 HRSOut statusbyte, noteUit, release ' gives a 1ms delay Case 120 To 126 HRSOut NoteOff_Status, noteUit, 0 ' for valve board Case 127 Clear Red EndSelect Set noteUit 'reset EndIf Case NoteOn_Status If noteAan = 255 Then noteAan = Bytein Else velo = Bytein If velo = 0 Then Select noteAan Case 52 To 94 HRSOut statusbyte, noteAan, velo Case 120 To 126 HRSOut statusbyte, noteAan, 0 Case 127 Clear Red EndSelect Else Select noteAan Case 52 To 94 HRSOut statusbyte, noteAan, velo ' for synth board Case 120 To 126 HRSOut statusbyte, noteAan, velo ' for valve board Case 127 Set Red EndSelect End If Set noteAan '= 255 'reset EndIf Case Keypres_Status 'used for lite flashing speed modulation If notePres = 255 Then notePres = Bytein Else pres = Bytein GoSub KeyPres Set notePres EndIf Case Control_Status 'this is where the action takes place for controllers If Ctrl = 255 Then Ctrl = Bytein Else value = Bytein Select Ctrl Case 1 ' implemented 09.12.2020 HRSOut Control_Status, 1, value Case 3 To 7 ' for the synth board : 3,4,5,6,7 HRSOut Control_Status, Ctrl, value Case 10 CC10 = value ' panning - Horizontal motor ' > 64 draait naar rechts ( gezien vanuit de bugel zelf) motor CW ' < 64 draait naar links , motor CCW ' first cancel any running command for Horizontal movement: Set Timvals[0] Set Timvals[1] Set Timvals[4] ' reset accelleration: H_acc = CC32 Clear H_Motor_Clock ' H_Sollpos = H_MinPos + ((CC10 * H_Traj) >> 7) ' can overflow!!! H_dw = CC10 * H_Traj ' max 2080768 H_dw = H_dw >> 7 ' = / 128 min 128 max 16256 H_Sollpos = H_MinPos + H_dw ' min 24704 to 40832 If H_Sollpos <= H_MinPos Then H_Sollpos = H_MinPos + H_Hysteresis If H_Sollpos >= H_MaxPos Then H_Sollpos = H_MaxPos - H_Hysteresis H_acc = CC32 Select H_Sollpos Case < H_MotPos ' we must go left... Clear H_Motor_Enable ' motor on If H_Left = 0 Then Clear H_Motor_Dir ' should be left or CW Set H_Left Clear H_Right EndIf Timvals[0] = time + H_MotorPeriod + H_acc Case > H_MotPos 'move right Clear H_Motor_Enable If H_Right = 0 Then Set H_Motor_Dir ' should be right or CCW Set H_Right Clear H_Left EndIf Timvals[1] = time + H_MotorPeriod + H_acc 'Case H_MotPos 'Set H_Motor_Enable 'ok motor blijft onbekrachtigd EndSelect Set Resort_flag Case 11 ' vertical tilt position for V_Motor CC11 = value ' V_Sollpos = CC11 << 1 ' make 8 bits ' better resolution by doing: V_Sollpos = CC11 + 64 ' becomes 64 to 191 - we may even go to 9bits... If V_Sollpos < MaxDown >> 2 Then V_Sollpos = MaxDown >> 2 If V_Sollpos > MaxUp >> 2 Then V_Sollpos = MaxUp >> 2 Set VelFlags.2 Timvals[2] = time + V_Period ' motor speed Set Resort_flag Case 13 ' ctrl. 13 used on the valve board only for fingerings. ' only the lowest 3 bits do matter HRSOut Control_Status, Ctrl, value & %00000111 Case 15 To 20 ' check this range if firmware is changed 16,17,18,19 ' adapted 09.12.2020, as CC15 and CC20 are implemented now. ' for the synth board HRSOut Control_Status, Ctrl, value Case 31 'can be used to change the speed of the horizontal motor 'MotorPeriod = 135 - value 'for 1/8 step mode 'better range gives: H_MotorPeriod = 95 - (value >>1) ' value = 0 - 63 ' so motorperiod = 95 - 32 'would be 3.2ms to 0.825ms Case 32 'can be used to modify the accelleration curve of the motor on starts. 'on startup CC32 is initialized to accelmax, or value 110. CC32 = value + 64 ' so 64 -> 191 H_acc = CC32 Case 33 If value > 112 Then value = 112 ' limit speed ' vertical motor speed V_Period = (127 - value) + 1 ' 128 --> 1 V_Period = V_Period << 3 ' 1024 --> 8 Case 66 ' on/off for the robot HRSOut Control_Status, 66, value If value = 0 Then Clear PowerOn ' CC66.0 GoSub PowerDown Else Set PowerOn ' CC66.0 Low V_Motor_Enable ' motor ON CC11 = V_EqPos >> 1 ' start with vertical position V_Sollpos = V_EqPos ' CC11 << 1 EndIf Case 69 ' eye-lights automation on or off. This is ON by default! HRSOut Control_Status, 69, value Case 100 To 108 ' waveform params for synth HRSOut Control_Status, Ctrl, value Case 123 HRSOut Control_Status, 123, value Low Red 'GoSub AllNotesOff EndSelect Set Ctrl EndIf Case ProgChange_Status If prog = 255 Then 'single byte message prog = Bytein HRSOut ProgChange_Status, prog ' for Synth board. Prog's 0-10 implemented ' bits 0,1,2,3 ' for valve board bits 4,5,6 are used EndIf Set prog Case Pitchbend_Status If pblsb = 255 Then pblsb = Bytein Else pbmsb = Bytein HRSOut Pitchbend_Status, pblsb, pbmsb ' for synth board Set pblsb EndIf EndSelect EndIf Check_Motors: ' check Vertical motor against soll values: If V_Sollpos <> V_NowVal Then If VelFlags.2 = 0 Then Set VelFlags.2 Timvals[2] = time + V_Period ' to be tested stepping rate Set Resort_flag EndIf Else If VelFlags.2 = 1 Then Clear VelFlags.2 Set Timvals[2] Set Resort_flag EndIf EndIf If Resort_flag = 1 Then GoSub SortTimers ' so we resort only if an incoming midi command or a motor process changed something EndIf Check_Timers: If idx < NrTasks Then ' we moeten alleen checken wanneer er een timer loopt If time >= Nxt Then ' nagaan of de eerstvolgende timer afgelopen is... ' in dit geval is de eerste timer afgelopen en moeten we de juiste aktie ondernemen: Set Nxt.31 ' timer reset, is immers afgelopen ' aan de hand van idx weten we welke timer dit is Select idx Case 0 'LEFT movement task for horizontal motor. 'by incrementing or decrementing the rsi (reschedule interval), we can 'make gentle speedups and slowdowns for motors, pwm's and fades. If L_sens = 1 Then ' end position reached If H_Right = 0 Then Set H_Motor_Dir ' set dir to R or CCW Set H_Right Clear H_Left EndIf Clear GreenLed Clear H_Motor_Enable ' power on Clear H_Motor_Clock H_Sollpos = H_MinPos + H_Hysteresis Timvals[1] = time + (H_MotorPeriod << 2) ' should be slow! Set H_Motor_Clock ' step now Inc H_MotPos Timvals[4] = time + 4 ' set pulse timer 4 = 200us Else ' we are not in touch with a sensor If H_MotPos = H_Sollpos Then ' stop task, as position is reached Clear GreenLed Set H_Motor_Enable ' disable motor Clear H_Motor_Clock ' clock low at rest Set Timvals[0] Set Timvals[1] Set Timvals[4] Else ' now we are certain that a step to the left is possible and required: Clear H_Motor_Enable ' enable motor If H_Left = 0 Then Clear H_Motor_Dir ' turn left, CW Set H_Left Clear H_Right EndIf Timvals[4] = time + 4 ' puls timer 'If H_MotPos > 0 Then Dec H_MotPos ' error condition Clear H_Remweg If H_acc > 0 Then Dec H_acc 'speedup counter If H_acc = 0 Then ' hier zijn we dus op snelheid gekomen ' maar nu moeten we vertragen in funtie van de nog af te leggen weg H_Remweg = Abs(H_MotPos - H_Sollpos) If H_Remweg <= CC32 Then ' we naderen het doel en moeten vertragen... H_Remweg = CC32 - H_Remweg Else Clear H_Remweg EndIf EndIf ' set the timer to reload task0: Timvals[0] = time + H_MotorPeriod + H_acc + H_Remweg Set H_Motor_Clock ' step now If H_MotPos > 0 Then Dec H_MotPos Else Set Red ' for debug EndIf EndIf EndIf Case 1 'turn horizontal motor to the right: If R_sens = 1 Then ' in touch with sensor If H_Left = 0 Then Clear H_Motor_Dir ' only allow left movement Set H_Left Clear H_Right EndIf Clear YellowLed Clear H_Motor_Enable ' we need power! H_Sollpos = H_MaxPos - H_Hysteresis ' Set Timvals[1] ' clear this timer as we must go left Timvals[0] = time + (H_MotorPeriod << 2) ' slow! Timvals[4] = time + 4 Set H_Motor_Clock ' pulse Dec H_MotPos Else If H_MotPos = H_Sollpos Then 'stop task, as position is reached Clear YellowLed Set H_Motor_Enable 'disable motor Clear H_Motor_Clock ' zero at rest Set Timvals[1] ' clear timer Set Timvals[0] Set Timvals[4] Else ' now we are certain that a step to the right is possible: Clear H_Motor_Enable ' motor on If H_Right = 0 Then Set H_Motor_Dir ' set to rotate right Set H_Right Clear H_Left EndIf Timvals[4] = time + 4 'If H_MotPos < 65535 Then Inc H_MotPos ' error condition Clear H_Remweg If H_acc > 0 Then Dec H_acc 'speedup counter If H_acc = 0 Then ' hier zijn we dus op snelheid gekomen ' maar nu moeten we vertragen in funtie van de nog af te leggen weg H_Remweg = Abs(H_MotPos - H_Sollpos) If H_Remweg <= CC32 Then ' we naderen het doel en moeten vertragen... H_Remweg = CC32 - H_Remweg Else Clear H_Remweg EndIf EndIf ' set the timer to reload task1: Timvals[1] = time + H_MotorPeriod + H_acc + H_Remweg 'add the period duration for the motor pulses Set H_Motor_Clock ' pulse now Inc H_MotPos EndIf EndIf Case 2 ' vertical motor positioning Clear V_Motor_Clock ' becomes high - steps happen on the negative edge for the Nanotech SMC42-2 controller ' CC11 is only 7 bit, V_Sollpos is 8 bit Select V_NowVal ' now in 8 bits Case V_Sollpos Clear V_Motor_Enable ' keep torque on motor Clear VelFlags.2 ' stop timer task Set Timvals[2] ' cancel timer Clear BlueLed Case > V_Sollpos ' huidige positie groter dan V_Sollpos Clear V_Motor_Dir Timvals[3] = time + 4 ' 10 = 250 us, 4 = 100us Set V_Motor_Clock ' step Set VelFlags.2 ' motor speed should be a function of the absolute value of the traject or fault If V_Period > Abs(V_NowVal - V_Sollpos) Then Timvals[2] = time + V_Period - Abs(V_NowVal - V_Sollpos) ' to be checked to get ca. 64Hz here ' 3128 geeft 75ms or 13.3Hz - 23us per unit ' 679 geeft 64 Hz ' this gives the motor speed or stepping rate Else Timvals[2] = time + V_Period EndIf Btg BlueLed Case < V_Sollpos ' seinpos < V_Sollpos Set V_Motor_Dir Timvals[3] = time + 4 Set V_Motor_Clock ' step Set VelFlags.2 If V_Period > Abs(V_NowVal - V_Sollpos) Then Timvals[2] = time + V_Period - Abs(V_NowVal - V_Sollpos) Else Timvals[2] = time + V_Period EndIf Btg BlueLed EndSelect Case 3 ' used as V_motor steppulse one-shot for the SMC42-2 Set Timvals[3] Clear V_Motor_Clock ' return clock to the rest state - High on output Case 4 ' used as H_motor steppulse one-shot for the Geckodrive Clear H_Motor_Clock ' so on the controller the clock/step pin will be at ground (0). Set Timvals[4] ' cancel one-shot timer If H_Left = 1 Then ' we just did a step to the left Clear PIE2BITS_TMR3IE ' disable sampling IRQ-TMR3 L_sensorVal = ADIn 1 Set PIE2BITS_TMR3IE ' enable sampling IRQ-TMR3 If L_sensorVal > 280 Then ' 00 And L_sensorVal < 350 Then Set L_sens ' sensor touched H_MinPos = H_MotPos H_MaxPos = H_MinPos + H_Traj If H_Sollpos < H_MinPos Then H_Sollpos = H_MinPos Set Timvals[0] 'Else ' Clear L_sens EndIf Else Clear L_sens EndIf If H_Right = 1 Then Clear PIE2BITS_TMR3IE ' disable sampling IRQ-TMR3 R_sensorVal = ADIn 2 Set PIE2BITS_TMR3IE ' enable sampling IRQ-TMR3 If R_sensorVal > 280 Then ' And R_sensorVal < 350 Then Set R_sens H_MaxPos = H_MotPos H_MinPos = H_MaxPos - H_Traj If H_Sollpos > H_MaxPos Then H_Sollpos = H_MaxPos Set Timvals[1] 'Else ' Clear R_sens EndIf Else Clear R_sens EndIf 'Case Else ' ' in dit geval is idx geset ' GoTo jumpout EndSelect GoSub SortTimers ' find a new nxt and idx EndIf Else ' idx > 5, no timers running, so to avoid overflows, we can reset the timer If MaxTim = 1 Then Clear Cnt Clear VelFlags Set Timvals EndIf EndIf 'Btg loopcnt ' for loopspeed measurement - 5us measured 12.12.2016 Loop ' end of the main loop SortTimers: 'look up the next smallest timer value in the Timvals array ' zoek de de volgende kleinste timer waarde: Set idx ' makes it 255 Set Nxt.31 ' nxt is set on entry. - for speed, we just set the highest bit For i = 0 To NrTasks -1 If Timvals[i] < Nxt Then Nxt = Timvals[i] ' NrTasks dword comparisons idx = i EndIf Next i Clear Resort_flag Return KeyPres: 'the note to which the pressure should be applied is passed in NotePres, the value in Pres ' Select notePres ' Case 120 To 126 ' HRSOut Notepress_Status, notePres, pres ' not implemented on the valve board ' EndSelect Set notePres '= 255 Return ProgChange: Set prog 'this is not realy required Return Pitchbend: 'only implemented on the synth board Set pblsb Return Aftertouch: 'this is the channel aftertouch, affecting all notes Set aft Return PowerDown: 'should return the motor to equilibrium position and than switch off Clear VelFlags0 'stop all running timers High V_Motor_Enable ' off Clear V_Motor_Clock ' on the controller this gives a 1 Clear V_Motor_Dir ' = 0 High H_Motor_Enable ' off Clear H_Motor_Clock '= 0 'Clear H_Motor_Dir '= 0 irrelevant here Low Red V_Period = V_Period_Default ' V_Sollpos = V_EqPos ' CC11 = V_EqPos >> 1 H_MotorPeriod = 80 ' default speed CC31 H_acc = 64 ' default acceleration CC32 Return H_Calibrate: Set Red ' first move to the left, until sensor is reached: Clear H_Motor_Enable ' motor ON DelayMS 1 Clear H_Motor_Dir ' move to the left Set H_Left Clear H_Right DelayMS 1 Clear H_Motor_Clock ' clock low at rest DelayMS 1 H_MotPos = 32768 ' midway H_speed = 20 Clear L_sens While L_sens = 0 L_sensorVal = ADIn 1 If L_sensorVal > 280 Then Set L_sens PulseOut H_Motor_Clock, 100 DelayMS H_speed ' 2ms delay, so step frequency = 500 Hz If H_speed > 2 Then Dec H_speed Dec H_MotPos Wend Set L_Calib H_MinPos = H_MotPos Clear L_sens ' now move to the right, until right sensor is reached: Set H_Motor_Dir ' move to the right Clear H_Left Set H_Right Clear R_sens H_speed = 20 DelayMS 1 While R_sens = 0 R_sensorVal = ADIn 2 If R_sensorVal > 280 Then Set R_sens PulseOut H_Motor_Clock, 100 DelayMS H_speed If H_speed > 2 Then Dec H_speed Inc H_MotPos Wend H_MaxPos = H_MotPos H_Traj = H_MaxPos - H_MinPos Set R_Calib ' now move back to the central position Clear H_Motor_Dir ' move to the left Set H_Left Clear H_Right H_Sollpos = H_MinPos + (H_Traj >> 1) H_speed = 26 DelayMS 1 While H_MotPos > H_Sollpos PulseOut H_Motor_Clock, 100 DelayMS H_speed If H_speed > 2 Then Dec H_speed Dec H_MotPos Wend ' we should be back to central position now Clear R_sens Clear L_sens Set H_Motor_Enable ' motor OFF H_Sollpos = H_MotPos Clear Red Return Dur_Lookup: ' copied from valve board firmware Set Dur[0] ' lookup for valve force (ctrl 25) ' 1 time unit here is 26us (measured 04.12.2016) Dur[1]= 23674 ' period = 616ms or freq = 1.622Hz Dur[2]= 22917 Dur[3]= 22548 ' freq= 2.21749157353202 Dur[4]= 22185 ' freq= 2.25377507324769 Dur[5]= 21827 ' freq= 2.29074082558299 Dur[6]= 21475 ' freq= 2.32828870779977 Dur[7]= 21129 ' freq= 2.36641582658905 Dur[8]= 20789 ' freq= 2.40511809129828 Dur[9]= 20454 ' freq= 2.44450963136795 Dur[10]= 20124 ' freq= 2.48459550785132 Dur[11]= 19800 ' freq= 2.52525252525252 Dur[12]= 19481 ' freq= 2.56660335711719 Dur[13]= 19167 ' freq= 2.60865028434288 Dur[14]= 18858 ' freq= 2.65139463357726 Dur[15]= 18554 ' freq= 2.69483669289641 Dur[16]= 18255 ' freq= 2.73897562311695 Dur[17]= 17961 ' freq= 2.7838093647347 Dur[18]= 17672 ' freq= 2.82933454051607 Dur[19]= 17387 ' freq= 2.87571173865532 Dur[20]= 17107 ' freq= 2.92278014847723 Dur[21]= 16831 ' freq= 2.97070881112233 Dur[22]= 16560 ' freq= 3.01932367149758 Dur[23]= 16293 ' freq= 3.06880255324372 Dur[24]= 16030 ' freq= 3.11915159076731 Dur[25]= 15772 ' freq= 3.17017499365965 Dur[26]= 15518 ' freq= 3.22206469905916 Dur[27]= 15268 ' freq= 3.27482315954938 Dur[28]= 15022 ' freq= 3.32845160431367 Dur[29]= 14780 ' freq= 3.382949932341 Dur[30]= 14542 ' freq= 3.43831660019254 Dur[31]= 14307 ' freq= 3.4947927587894 Dur[32]= 14077 ' freq= 3.55189315905378 Dur[33]= 13850 ' freq= 3.6101083032491 Dur[34]= 13627 ' freq= 3.66918617450649 Dur[35]= 13407 ' freq= 3.72939509211606 Dur[36]= 13191 ' freq= 3.79046319460238 Dur[37]= 12978 ' freq= 3.85267375558638 Dur[38]= 12769 ' freq= 3.91573341686898 Dur[39]= 12564 ' freq= 3.97962432346386 Dur[40]= 12361 ' freq= 4.04498017959712 Dur[41]= 12162 ' freq= 4.1111659266568 Dur[42]= 11966 ' freq= 4.17850576633796 Dur[43]= 11773 ' freq= 4.24700586086809 Dur[44]= 11583 Dur[45]= 11397 ' freq= 4.38711941739054 Dur[46]= 11213 ' freq= 4.45910996165165 Dur[47]= 11032 ' freq= 4.53226976069616 Dur[48]= 10855 ' freq= 4.60617227084293 Dur[49]= 10680 ' freq= 4.6816479400749 Dur[50]= 10508 ' freq= 4.75827940616673 Dur[51]= 10338 ' freq= 4.83652544012381 Dur[52]= 10172 ' freq= 4.91545418796697 Dur[53]= 10008 ' freq= 4.99600319744204 Dur[54]= 9846 ' freq= 5.07820434694292 Dur[55]= 9688 ' freq= 5.16102394715111 Dur[56]= 9532 ' freq= 5.24548887956357 Dur[57]= 9378 ' freq= 5.33162721262529 Dur[58]= 9227 ' freq= 5.4188793757451 Dur[59]= 9078 ' freq= 5.50782110597048 Dur[60]= 8932 ' freq= 5.59785042543663 Dur[61]= 8788 ' freq= 5.68957669549385 Dur[62]= 8646 ' freq= 5.78302105019662 Dur[63]= 8507 Dur[64]= 8370 Dur[65]= 8235 ' freq= 6.07164541590771 Dur[66]= 8102 ' freq= 6.17131572451246 Dur[67]= 7972 ' freq= 6.27195183140993 Dur[68]= 7843 ' freq= 6.37511156445238 Dur[69]= 7717 ' freq= 6.47920176234288 Dur[70]= 7593 ' freq= 6.58501251152377 Dur[71]= 7470 ' freq= 6.69344042838019 Dur[72]= 7350 ' freq= 6.80272108843537 Dur[73]= 7231 ' freq= 6.91467293597013 Dur[74]= 7115 ' freq= 7.02740688685875 Dur[75]= 7000 ' freq= 7.14285714285714 Dur[76]= 6888 ' freq= 7.25900116144018 Dur[77]= 6777 ' freq= 7.37789582411096 Dur[78]= 6667 ' freq= 7.49962501874906 Dur[79]= 6560 ' freq= 7.62195121951219 Dur[80]= 6454 ' freq= 7.74713356058258 Dur[81]= 6350 ' freq= 7.8740157480315 Dur[82]= 6248 ' freq= 8.00256081946223 Dur[83]= 6147 ' freq= 8.13404912965674 Dur[84]= 6048 ' freq= 8.26719576719577 Dur[85]= 5951 ' freq= 8.40194925222652 Dur[86]= 5855 ' freq= 8.5397096498719 Dur[87]= 5760 ' freq= 8.68055555555555 Dur[88]= 5668 ' freq= 8.82145377558222 Dur[89]= 5576 ' freq= 8.96700143472023 Dur[90]= 5486 ' freq= 9.11410864017499 Dur[91]= 5398 ' freq= 9.26268988514264 Dur[92]= 5311 ' freq= 9.41442289587648 Dur[93]= 5225 ' freq= 9.56937799043062 Dur[94]= 5141 ' freq= 9.72573429293912 Dur[95]= 5058 Dur[96]= 4977 Dur[97]= 4897 Dur[98]= 4818 Dur[99]= 4740 Dur[100]= 4664 Dur[101]= 4589 Dur[102]= 4515 Dur[103]= 4442 Dur[104]= 4370 Dur[105]= 4300 Dur[106]= 4231 Dur[107]= 4162 Dur[108]= 4095 Dur[109]= 4029 Dur[110]= 3964 Dur[111]= 3901 Dur[112]= 3838 Dur[113]= 3776 Dur[114]= 3715 Dur[115]= 3655 Dur[116]= 3596 Dur[117]= 3538 Dur[118]= 3481 Dur[119]= 3425 Dur[120]= 3370 Dur[121]= 3316 Dur[122]= 3262 Dur[123]= 3210 Dur[124]= 3158 Dur[125]= 3107 Dur[126]= 3057 Dur[127]= 3008 ' 04.12.2016: Bug code: Dur[127] geeft freq = 6.386 Hz, period = 156.6ms ' = 75ms to 220ms ' this is way too much, so we divide by 16: For i = 0 To 127 Dur[i] = Dur[i] >> 4 ' the range is now 5ms to 38ms Next i Set Dur5[0] ' used for light flashing and valve release time Dur5[1]= 41667 ' = 1 second , full period = 2 seconds, freq = 0.5Hz Dur5[2]= 39494 ' freq= 1.0550125757499 Dur5[3]= 38450 ' freq= 1.08365843086259 Dur5[4]= 37434 ' freq= 1.11307011451265 Dur5[5]= 36445 ' freq= 1.14327525495038 Dur5[6]= 35482 ' freq= 1.17430434210774 Dur5[7]= 34544 ' freq= 1.20619113787247 Dur5[8]= 33631 ' freq= 1.23893629885126 Dur5[9]= 32743 ' freq= 1.27253662360403 Dur5[10]= 31877 ' freq= 1.30710752789367 Dur5[11]= 31035 ' freq= 1.34257021642232 Dur5[12]= 30215 ' freq= 1.37900601246621 Dur5[13]= 29416 ' freq= 1.41646269603844 Dur5[14]= 28639 ' freq= 1.45489251254117 Dur5[15]= 27882 ' freq= 1.49439303732396 Dur5[16]= 27146 ' freq= 1.53490999287802 Dur5[17]= 26428 ' freq= 1.57661066545583 Dur5[18]= 25730 ' freq= 1.61938074880166 Dur5[19]= 25050 ' freq= 1.66333998669328 Dur5[20]= 24388 ' freq= 1.70849051446066 Dur5[21]= 23743 ' freq= 1.75490319953951 Dur5[22]= 23116 ' freq= 1.8025033166061 Dur5[23]= 22505 ' freq= 1.85144042064726 Dur5[24]= 21910 ' freq= 1.90171915411532 Dur5[25]= 21331 ' freq= 1.95333864641445 Dur5[26]= 20768 ' freq= 2.006291730868 Dur5[27]= 20219 ' freq= 2.06076792455941 Dur5[28]= 19685 ' freq= 2.11667090000847 Dur5[29]= 19164 ' freq= 2.17421554303207 Dur5[30]= 18658 ' freq= 2.23317969057062 Dur5[31]= 18165 ' freq= 2.29378842095605 Dur5[32]= 17685 ' freq= 2.35604561304307 Dur5[33]= 17218 ' freq= 2.41994811631239 Dur5[34]= 16763 ' freq= 2.48563304102289 Dur5[35]= 16320 ' freq= 2.5531045751634 Dur5[36]= 15888 ' freq= 2.62252433702585 Dur5[37]= 15469 ' freq= 2.69355916133342 Dur5[38]= 15060 ' freq= 2.76671093404161 Dur5[39]= 14662 ' freq= 2.84181330423316 Dur5[40]= 14274 ' freq= 2.91906029610948 Dur5[41]= 13897 ' freq= 2.99824902257082 Dur5[42]= 13530 ' freq= 3.07957625030796 Dur5[43]= 13172 ' freq= 3.16327563518575 Dur5[44]= 12824 ' freq= 3.24911624038262 Dur5[45]= 12485 ' freq= 3.33733813910025 Dur5[46]= 12155 ' freq= 3.42794460441519 Dur5[47]= 11834 ' freq= 3.52092839840009 Dur5[48]= 11522 ' freq= 3.61627032343922 Dur5[49]= 11217 ' freq= 3.71459986330272 Dur5[50]= 10921 ' freq= 3.81527943106553 Dur5[51]= 10632 ' freq= 3.91898670679709 Dur5[52]= 10351 ' freq= 4.02537597011561 Dur5[53]= 10078 ' freq= 4.13441820467024 Dur5[54]= 9811 ' freq= 4.24693371385859 Dur5[55]= 9552 ' freq= 4.36208821887214 Dur5[56]= 9300 ' freq= 4.48028673835125 Dur5[57]= 9054 ' freq= 4.60201752448273 Dur5[58]= 8815 ' freq= 4.72679145396105 Dur5[59]= 8582 ' freq= 4.85512312592247 Dur5[60]= 8355 ' freq= 4.9870337123479 Dur5[61]= 8134 ' freq= 5.12253094008688 Dur5[62]= 7919 ' freq= 5.26160710527423 Dur5[63]= 7710 ' freq= 5.40423692174665 Dur5[64]= 7506 ' freq= 5.5511146638245 Dur5[65]= 7308 ' freq= 5.70151432220398 Dur5[66]= 7115 ' freq= 5.85617240571562 Dur5[67]= 6927 ' freq= 6.01510995621 Dur5[68]= 6744 ' freq= 6.1783313562673 Dur5[69]= 6565 ' freq= 6.34678852500635 Dur5[70]= 6392 ' freq= 6.51856487275761 Dur5[71]= 6223 ' freq= 6.69559162247576 Dur5[72]= 6059 ' freq= 6.87682235792485 Dur5[73]= 5898 ' freq= 7.06454165253758 Dur5[74]= 5743 ' freq= 7.25520924023449 Dur5[75]= 5591 ' freq= 7.45245334764204 Dur5[76]= 5443 ' freq= 7.65509216730969 Dur5[77]= 5299 ' freq= 7.86311882745172 Dur5[78]= 5159 ' freq= 8.07650061381405 Dur5[79]= 5023 ' freq= 8.29517552591413 Dur5[80]= 4890 ' freq= 8.52079072937969 Dur5[81]= 4761 ' freq= 8.75166281593503 Dur5[82]= 4635 ' freq= 8.98957209636821 Dur5[83]= 4513 ' freq= 9.23258734027624 Dur5[84]= 4393 ' freq= 9.48478640261021 Dur5[85]= 4277 ' freq= 9.74203101862676 Dur5[86]= 4164 ' freq= 10.0064040986231 Dur5[87]= 4054 ' freq= 10.277914816642 Dur5[88]= 3947 ' freq= 10.5565408326999 Dur5[89]= 3843 ' freq= 10.842223956978 Dur5[90]= 3741 ' freq= 11.1378419317473 Dur5[91]= 3642 ' freq= 11.4406004027091 Dur5[92]= 3546 ' freq= 11.7503290092122 Dur5[93]= 3452 ' freq= 12.0702974121282 Dur5[94]= 3361 ' freq= 12.3971040364971 Dur5[95]= 3272 ' freq= 12.7343113284434 Dur5[96]= 3186 ' freq= 13.0780498012136 Dur5[97]= 3102 ' freq= 13.4321942832581 Dur5[98]= 3020 ' freq= 13.7969094922737 Dur5[99]= 2940 ' freq= 14.172335600907 Dur5[100]= 2862 ' freq= 14.5585837409737 Dur5[101]= 2787 ' freq= 14.9503647889008 Dur5[102]= 2713 ' freq= 15.3581521071385 Dur5[103]= 2641 ' freq= 15.7768522024486 Dur5[104]= 2571 ' freq= 16.2064047711656 Dur5[105]= 2504 ' freq= 16.640042598509 Dur5[106]= 2437 ' freq= 17.0975242784845 Dur5[107]= 2373 ' freq= 17.5586458772299 Dur5[108]= 2310 ' freq= 18.037518037518 Dur5[109]= 2249 ' freq= 18.5267526307989 Dur5[110]= 2190 ' freq= 19.0258751902588 Dur5[111]= 2132 ' freq= 19.5434646654159 Dur5[112]= 2076 ' freq= 20.0706486833654 Dur5[113]= 2021 ' freq= 20.616856341745 Dur5[114]= 1967 ' freq= 21.182850364345 Dur5[115]= 1915 ' freq= 21.7580504786771 Dur5[116]= 1865 ' freq= 22.3413762287757 Dur5[117]= 1815 ' freq= 22.9568411386593 Dur5[118]= 1767 ' freq= 23.5804565176382 Dur5[119]= 1721 ' freq= 24.2107301956227 Dur5[120]= 1675 ' freq= 24.8756218905473 Dur5[121]= 1631 ' freq= 25.5466993664418 Dur5[122]= 1588 ' freq= 26.2384550797649 Dur5[123]= 1546 ' freq= 26.9512721000431 Dur5[124]= 1505 ' freq= 27.6854928017719 Dur5[125]= 1465 ' freq= 28.4414106939704 Dur5[126]= 1427 ' freq= 29.1987853305302 Dur5[127]= 1389 ' = 36ms, full period = 72ms, freq.= 13.8 Hz Return '[EOF]