'**************************************************************** '* Name : Fa_Motor.BAS * '* Author : Godfried-Willem RAES * '* Notice : Copyleft (c) 2016 Logosoft Public Domain * '* Date : 08-11-2016 * '* Version : 1.3 * '* Notes : Based On Fa-hub code model, V3.1 * '**************************************************************** '18.10.2010: Further coding for Ob. ' Analog 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 '----------------------------------------------------------- '20.04.2011: Ob code, version 3.1 used as model for the Fa hub board ' pin assignations are a bit different though. Ob must have an earlier board version... ' motor parameters have to be changed. Its a very different mechanism after all. '26.04.2011: Start works on the motor coding. ' CC21 sets the minimum speed for the motor ' CC22 sets the soll-position (destination) ' CC23 sets the maximum speed for the motor '30.04.2011: Implemented: Timer3 as clock source for periodic sensor sampling at 64S/s '01.05.2011: Version 1.4: with sampling in the low priority interrupt. '21.05.2011: minimal integration added in low interupt on ADC. Motor movement code changed, leaving out ' angle calculations. Now version 1.5 '25.05.2011: Further work on motor coding. ' ------------------------------------------------------------------------------------------------------ '02.11.2016: New motor control board for finished. ' start new coding. This firmware does handle nothing but the motor and movement. ' Sampling in the timer3 interrupt at 64S/s '04.11.2016: Trying to automate movement in function of the playing. ' this may cause a lot of clicking noise from the relays though... ' still not working properly - we had a bug in left adjust AD data... ' Now works in principle. Polarities to be carefully checked! ' Automove implemented. The higher the pitches played become, the more forward the bassoon will move. ' This feature is switched on with controller 67, a switch. ' NOTE: Motor running CW makes the bassoon move forwards ' CCW, backwards to the reclined position '05.11.2016: Maybe a wait time before direction changes should be implemented. '06.11.2016: waittime is not required as long the the motor procedure runs slower than the ' RC time on the PWM (<15ms) and the switch time for the relays. ' We should have provided many more LED's on the board for testing and debugging... '07.11.2016: Further coding and testing with a spare motor, sensor and power supply. '08.11.2016: Boards mounted in Fa. First tests... ' Minpos has to be smaller. Fully reclined measures 1.76 V on the sensor. ' Oscillations do occur, except on minpos. ' Fine tuning required. Maybe we need a voltage burst everytime sollpos changes. '09.11.2016: timer coding added but not yet used. ' should be implemented to give a short pwm boost to the motor on each change of ' direction of rotation. ' Torque regulation added. Now movement should always be as slow as possible. '10.11.2016: Strange compiler bug: Symbol not previously defined PBP#VAR0HH ... Include "18F2525.inc" 'version for the fa board. (40MHz) 'Include "18F2520.inc" 'also possible. (40MHz) ' Mapping defines for midi-events on pin outputs and inputs: $define CW_Relay PORTA.2 $define CCW_Relay PORTA.3 $define BlueLed PORTB.2 ' position reached indicator. $define GreenLed PORTC.2 ' for code monitoring and debug - movement to the left (backwards) $define YellowLed PORTC.3 ' for code monitoring and debug - movement to the right (forwards) $define Sensor PORTA.0 ' input port - Penny& Giles tilt sensor $define Motor PORTC.1 ' hpwm 2 $define Sampling PORTC.0 ' for sampling rate measurement and debug (64S/s) $define Loopspeed PORTC.4 ' for loopspeed measurement $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 = %01000011 '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 'initialisation of analog input channel Declare Adin_Res = 10 ' 10-bit result required Declare Adin_Stime = 50 ' Allow 50us sample time ADCON2 = %10001010 ' right justified,0_TAD, Fosc/32 ADCON1 = %00001110 ' A0 is analog input , cfr. Microchip manual p.224 ADCON0 = %00000001 ' bits 5-2 =0 selects AN0 , bit 1 is the ready bit. Can be read. 'constant definitions: 'initialisations for the midi input parser: Symbol Midichannel = 15 ' Fa_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 = 2 ' maximum 16 Symbol PWMFreq = PWMminF * 8 ' 19536Hz Symbol SensorMin = 360 ' 360 = 1.76V op sensor ' 401 = 1.96V op sensor, waarde 401 in 10 bits resolutie, = ca 10 degrees backwards Symbol SensorMax = 592 ' 2.89V op sensor, waarde 592 in 10 bits resolutie, = 10 degrees frontal Symbol MinPos = 0 Symbol MaxPos = SensorMax - SensorMin ' = 232 '=191 Symbol EqPos = 461 - SensorMin ' = 101 , equilibrium position, slightly reclined. 2.25V op sensor ' 10-bit reading as 461 ' value checked empirically. Symbol MaxTraj = MaxPos - MinPos ' = 232 ' = 191 Symbol VoorMaxTraj = MaxPos - EqPos ' = 131 Symbol AchterMaxTraj = EqPos - MinPos ' = 101 '= 60 Symbol CC21_default = 1 ' startup and reset midi value for minimum motor speed Symbol CC22_default = 0 ' fully reclined on startup and reset - sollpos! Symbol CC23_default = 127 ' startup and reset midi value for maximum speed Symbol Backwards = 0 ' constants for direction of movement Symbol still = 64 Symbol Forwards = 127 ' 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!!! ' 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 Tim3 As TMR3L.Word ' 16 bit counter for sampler ' Dim Sr as TMR0L.7 '512 S/s ' As TMR0H.1 would be 128 S/s ' As TMR0H.2 would be 64 S/s ' As TMR0H.3 would be 32 S/s ' As TMR0H.4 would be 16 S/s 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 veltim As Dword System ' 32 bit velo Dim newtim As Dword System 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 time As Dword System ' 32-bit, incremented in loopcounter Dim maxtim As time.31 ' overflow bit, will cause timer reset after 1h45 Dim t As Byte System ' loopcounter, running at 4 times time-clock Dim tog As Byte System Dim tg As tog.0 ' divide by 4 bit Dim TimVals[NrTasks] As Dword ' lijst met timer waarden - de kleinste is eerst aan de beurt Dim Nxt As Dword 'System ' waarde voor de eerstvolgende timer Dim idx As Byte System ' index voor de eerstvolgende timer Dim CC21 As Byte 'System ' motor minimum speed pwm - midi value Dim CC22 As Byte System ' motor soll-position - midi value Dim CC23 As Byte 'System ' motor maximum speed pwm - midi value Dim CC66 As Byte System ' global on/off switch Dim PowerOn As CC66.0 Dim st As Byte System Dim b1 As Byte System Dim b2 As Byte System ' Dim Lites As Byte System ' bits used as flags Dim SensorVal As Word System ' holds the 10bit value read from ADC0 (integrated) ' in fact 8 bit could also do the job here. Dim nowval As Word System ' only used in irq to read the sensorvalue 10 bits Dim oldpos As Byte System Dim nowpos As Byte System Dim Sollpos As Byte System ' 8 bit = CC22 * 2 Dim tBit As Byte System ' for sampling rate bit Dim MinSpeed As Byte 'System ' 8 bit minimum pwm value for the motor to rotate Dim MaxSpeed As Byte System ' 8 bit maximum pwm value for the motor Dim Traj As Byte System ' watch out for sign!!! Dim Speed As Word System ' actual motor speed - 8-bit - word for calculations Dim torque As Byte System Dim pitch As Byte 'System ' averaged playing pitch for automove Dim CC67 As Byte 'System Dim automove As CC67.0 Dim flags As Byte System Dim silent As flags.0 ' zero when no note is sounding, else 1 Dim stalling As flags.1 Dim Direction As Byte 'System ' direction of movement = Backwards, still or Forwards ' Dim olddir As Byte System '----------------------------------------------------------------------------------------- ' variables for the IRQ handler: Dim PP0 As Byte System ' \ Dim PP0H As Byte System ' / Storage for FSR0L\H registers Dim FSR0SaveInt As PP0 ' alias Dim FSR0SaveIntH As PP0H ' alias Dim USART_FSR0_Save As FSR0SaveInt.Word ' Alias the FSR0L\H storage to PP0\H Dim USART_FSR1_Save As Word System ' Storage for FSR1L\H registers Dim USART_FSR0 As FSR0L.Word ' alias Dim USART_FSR1 As FSR1L.Word ' alias Dim IndexIn As Byte System ' Pointer to the next empty location in the buffer Dim IndexOut As Byte System ' Pointer to the location of the oldest character in the buffer Dim Ringbuffer[256] As Byte ' Load the USART Interrupt handler And buffer read subroutines into memory Include "Fa_Mot_Irq.inc" ' our own version for UART And Timer0/3 Interrupt Dim Vels[128] As Word ' for Laukhuff solenoid 'framework for a multitasker: 'Dim Task_rsi[NrTasks] As Word 'task reschedule interval (period), if 0 the task is not active ' 'max. value limited to 65535. For longer periods, it will have to ' 'become dword!!! 'Dim Velmsb[NrTasks] As Word 'the application for velo-timers, is in fact just a one-shot task 'Dim VelLsb[NrTasks] As Word 'make sure we initialize those pins on start up: Low CW_Relay ' A2 - relay1 High CCW_Relay ' A3 - relay2 Low Debug_Led HPWM 2, 0, PWMFreq 'motor speed - RC1 PWM uses timer2 Low GreenLed Low YellowLed Low BlueLed Clear CC66 'power off Clear CC67 Clear pitch CC21 = CC21_default 'minimum motor speed CC22 = CC22_default 'thus on cold boot, the bassoon will go to the fully reclined position CC23 = CC23_default 'maximum motor speed Clear Sollpos '= CC22 << 1 ' = minpos MinSpeed = CC21 ' 07.11.2016 was: << 1 ' cc21 * 2 MaxSpeed = CC23 << 1 ' MinSpeed + CC23 + MaxTraj 'cc23 = 7bits ' maxtraj = 96 '----------------------------------------------------------------------------------------- ' Main program starts here MAIN: High Debug_Led DelayMS 10 ' wait for stability Low Debug_Led 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 bot, 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 the ADC: SensorVal = ADIn 0 ' initialize with the value on startup - 10 bits ' begrenzing: If SensorVal < SensorMin Then SensorVal = SensorMin If SensorVal > SensorMax Then SensorVal = SensorMax nowpos = SensorVal - SensorMin ' 8 bits ' open and start timer3 for sampling: 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 Clear VelFlags Set TimVals GoSub Vels_Lookup ' start the main program loop: LOOP: Inc t ' byte If t.1 = tg Then Btg tg Inc time ' dword EndIf ' Create an infinite loop Bytein = HRSIn ' Read data from the serial buffer, with no timeout ' Start the midi parser. Midi_Parse: If Bytein > Control_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_Timers 'throw away... Else Clear statusbyte 'reset the status byte GoTo Check_Timers 'throw away EndIf 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 - no longer. 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 ' prog = 255 ' Case Aftertouch_Status ' statusbyte = Bytein ' aft = 255 ' Case Pitchbend_Status ' statusbyte = Bytein ' pblsb = 255 ' pbmsb = 255 EndSelect Else 'midi byte is 7 bits Select statusbyte Case 0 'not a message for this channel GoTo Check_Timers 'disregard Case NoteOff_Status If noteUit = 255 Then noteUit = Bytein Else release = Bytein 'message complete, so we can do the action... Select noteUit Case 34 To 91 '79 If pitch > 0 Then Dec pitch Set silent EndSelect Set noteUit 'reset EndIf GoTo Check_Timers Case NoteOn_Status If noteAan = 255 Then noteAan = Bytein Else velo = Bytein If velo = 0 Then Select noteAan Case 34 To 91 If pitch > 0 Then Dec pitch Set silent EndSelect Set noteAan 'reset !!! GoTo Check_Timers 'jump out Else Select noteAan Case 34 To 91 pitch = pitch + pitch + pitch + (noteAan - 33) ' max. = 58 pitch = pitch >> 2 ' integrated pitch value used for automove Clear silent EndSelect EndIf Set noteAan 'reset EndIf GoTo Check_Timers Case Keypres_Status 'used for lite flashing speed modulation If notePres = 255 Then notePres = Bytein Else pres = Bytein ' GoSub KeyPres EndIf Set notePres GoTo Check_Timers Case Control_Status 'this is where the action takes place for controllers If Ctrl = 255 Then Ctrl = Bytein Else value = Bytein GoSub Controller EndIf GoTo Check_Timers ' Case ProgChange_Status ' If prog = 255 Then 'single byte message ' prog = Bytein 'weak coding... ' GoSub ProgChange ' EndIf EndSelect EndIf ' timers: resort: GoSub SortTimers ' so we resort only if an incoming midi command changed something 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 ' notes 96,97,98 Laukhuff solenoid ' If notes.0 = 0 Then ' note off Clear VelFlags.0 ' Clear SolL ' Clear SolR Set TimVals[0] ' Else ' note is active ' If Velflags.0 = 1 Then ' velo timer afgelopen ' If LR0 = 0 Then ' staat links ' Clear SolL ' ' hold is switched off on reception of a note off command ' ' blijft links ' Else ' Clear SolR ' ' blijft rechts ' EndIf ' TimVals[0] = time + Rate0 ' wachttijd gaat in ' Clear Velflags.0 ' Else ' wachttijd is afgelopen ' If LR0 = 0 Then ' staat links ' Set SolR ' ga naar rechts ' Else ' staat rechts ' Set SolL ' ga naar links ' EndIf ' Toggle LR0 ' huidige positie ' TimVals[0] = time + velo0 ' stel een nieuwe velo-timer in ' Set Velflags.0 ' aktiveer hem ook ' EndIf ' EndIf Case 1 If VelFlags.1 = 1 Then ' velo time afgelopen Clear VelFlags.1 Set TimVals[1] EndIf 'Case Else ' ' in dit geval is idx geset ' GoTo jumpout EndSelect GoSub SortTimers ' find a new nxt and idx EndIf ' beveiliging tegen overflow crashes... If maxtim = 1 Then Clear time Clear VelFlags 'Clear notes Set TimVals EndIf Else ' idx > 5, no timers running, so to avoid overflows, we can reset the loop timer If maxtim = 1 Then Clear time ' 16.06.2015 EndIf EndIf Motor_Check: Cnt.Word0 = CntLw ' read counter If Cnt.10 <> tBit Then ' Cnt.14 sets the refresh rate at 4 S/s ' Cnt.13 at 8 S/s ' Cnt.12 at 16 S/s ' Cnt.11 at 32 S/s ' Cnt.10 at 64 S/s ' was Cnt.12 in code version 2.0 on tBit = Cnt.10 ' must be slower than RC time on PWM and switch time for the relays ' with Cnt.10 we have 15.6 ms. ' Coding without integration: ' If nowpos <> oldpos Then ' for monitoring sensor readout via midi-out ' HRSOut 160 + Midichannel, 60, nowpos >> 1 ' oldpos = nowpos ' EndIf If automove = 1 Then Sollpos = pitch << 2 ' with << 1 range becomes 0 - 116, we could do << 2 as well (0 - 232) If Sollpos > MaxPos Then Sollpos = MaxPos EndIf EndIf ' automated motor force regulation procedure: If nowpos = oldpos Then ' no motor movement detected If nowpos <> Sollpos Then ' although it should be moving... Set stalling ' motor is stalling If torque < 253 Then Inc torque ' increment motor force Inc torque EndIf Else ' position reached Clear torque ' reset force Clear stalling EndIf Else oldpos = nowpos ' motor is causing movement Clear stalling If torque > 1 Then Dec torque ' this will always cause the movement to be as slow as possible EndIf EndIf ' schmitt trigger around sollpos: If nowpos = Sollpos -1 Then nowpos = Sollpos EndIf If nowpos = Sollpos + 1 Then nowpos = Sollpos EndIf ' now compare this with the Soll-value Select nowpos Case Sollpos ' Requested position reached: switch off motor power. THIS WORKS ' the gears should be strong enough to hold the bassoon in place. CW_Relay = ~ CCW_Relay ' if ports have opposite signs, the motor is unpowered HPWM 2, 0, PWMFreq ' motorspeed zero Clear GreenLed Clear YellowLed Set BlueLed Direction = still ' Case Sollpos -1 To Sollpos + 2 ' CC22 * 2 ' 'Requested position reached: switch off motor power. THIS WORKS ' 'the gears should be strong enough to hold the bassoon in place. ' CW_Relay = ~ CCW_Relay ' if ports have opposite signs, the motor is unpowered ' HPWM 2, 0, PWMFreq ' motorspeed zero ' Clear GreenLed ' Clear YellowLed ' Set BlueLed ' Direction = still Case > Sollpos ' huidige positie groter dan sollpos If Direction = Forwards Then ' er was een verandering van draairichting... Clear torque ' dus bouwen we weer kracht op vanaf nul EndIf 'motor CCW - achterwaarts draaien 'sensor values must go down. 'motor speed should be function of length of traject to go Set CCW_Relay ' A3 - relay2 Set CW_Relay ' A2 - relay1 ' in geval de vorige richting nu Forwards was, kunnen we ' een getimede stroomstoot geven om de motor wat extra ' ' aanzetkracht te geven. ' If nowpos < EqPos Then ' 'falling backwards ' 'motorforce can be small, as weight helps movement ' ' Traj = EqPos - nowpos ' 59 --> 1 ' ' Traj = Traj >> 1 ' /2 ' 29 --> 0 ' ' Speed = MinSpeed + Traj ' 21.05.2011 ' ' changed 08.11.2016 ' Speed = MinSpeed >> 1 ' Else ' 'motor will be climbing, motorforce should be large ' Traj = nowpos - EqPos ' 1 --> 131 ' 'Speed = MaxSpeed - (VoorMaxTraj - Traj) ' 96 = maximaal trajekt ' 'Speed = MinSpeed + Traj ' can overflow if Minspeed > 124 !!! ' ' so CC21 should be limited in the controller procedure ' ' oscillates! ' ' change 09.11.2016: ' Speed = MinSpeed + (VoorMaxTraj - Traj) ' 96 = maximaal trajekt to be tested!!! ' EndIf Speed = torque If Speed > MaxSpeed Then Speed = MaxSpeed EndIf HPWM 2, Speed, PWMFreq ' motor Clear GreenLed Clear BlueLed Set YellowLed Direction = Backwards Case Else ' seinpos < sollpos If Direction = Backwards Then Clear torque EndIf Clear CCW_Relay Clear CW_Relay ' de fagot moet naar voor bewegen motor runs CW ' If nowpos < EqPos Then ' 'motor will be climbing ' 'motorforce must be medium high ' Traj = EqPos - nowpos ' 0 --> 60 ' 'Speed = MaxSpeed - (AchterMaxTraj - Traj) ' 22.05.2011 ' Speed = MinSpeed + Traj '07.11.2016 ' Else ' nowpos > 60 ' 'motor will be falling 'regardless the value of CC22 ' 'motorforce can be low ' ' Traj = nowpos - EqPos ' 0 --> 131 ' ' Traj = Traj >> 1 ' 0 --> 65 ' ' Speed = MinSpeed + Traj ' cannot overflow ' ' test 8.11.2016: ' Speed = MinSpeed >> 1 ' EndIf Speed = torque ' speed + torque If Speed > MaxSpeed Then Speed = MaxSpeed EndIf HPWM 2, Speed, PWMFreq ' motor Set GreenLed Clear YellowLed Clear BlueLed Direction = Forwards EndSelect If silent = 1 Then ' flag set/cleared on noteOn/Noteoff If pitch > 0 Then Dec pitch ' thus the robot will always return to the reclined polition ' when it is not playing. EndIf 'Btg Loopspeed ' ca. 20Hz may still be too fast , changed to 10Hz 7.11.2016 EndIf Btg Loopspeed ' 3.4us per lus . freq= ca 300kHz Tektronix measurement 04.11.2016 GoTo 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] ' 2 dword comparisons idx = i EndIf Next i Return ProgChange: Set prog '= 255 'this is not realy required Return Pitchbend: 'only implemented on dsPIC based robots Set pblsb '= 255 Return Aftertouch: 'this is the channel aftertouch, affecting all notes Set aft '= 255 'not mandatory Return Controller: Select Ctrl Case 21 ' ' maybe it would be better not to allow users to send this controller.. ' CC21 = value ' ' for development we use this controller to steer the minimum speed required ' ' for the motor to run. ' MinSpeed = CC21 << 1 'value * 2 ' ' we should recalculate maxspeed as well here... ' If MaxSpeed <= MinSpeed Then MaxSpeed = MinSpeed + 1 ' CC23 = MaxSpeed >> 1 ' we better leave this value 7 bits... 07.11.2016 ' as we add the traject to this value in the motor control loop CC21 = value If CC21 > 123 Then CC21 = 123 MinSpeed = CC21 If MaxSpeed <= MinSpeed Then MaxSpeed = MinSpeed + 1 CC23 = MaxSpeed << 1 EndIf Case 22 If PowerOn = 1 Then ' positie controller CC22 = value ' 7 bit Sollpos = CC22 << 1 ' make 8 bits 0 - 254 ' If Sollpos < MinPos Then Sollpos = MinPos ' 0 to 191 for 20 degree traject If Sollpos > MaxPos Then Sollpos = MaxPos Else Sollpos = MinPos EndIf CC22 = Sollpos >> 1 ' save the real 7 bit value Case 23 'for code development only CC23 = value ' leave this one 7 bits if speed needs to be limited MaxSpeed = CC23 << 1 ' 0-254 If MaxSpeed <= MinSpeed Then MaxSpeed = MinSpeed + 1 CC23 = MaxSpeed >> 1 Case 66 'on/off for the robot If value = 0 Then Clear PowerOn 'CC66.0 GoSub PowerDown Else Set PowerOn 'CC66.0 Sollpos = MinPos 'CC22 = Sollpos >> 1 Clear CC22 EndIf Case 67 If PowerOn = 1 Then CC67 = value If value > 0 Then Set automove ' bit 0 van CC67 Else Clear automove Clear pitch EndIf EndIf Case 123 Clear pitch Sollpos = MinPos 'CC22 = Sollpos >> 1 Clear CC22 EndSelect Set Ctrl '= 255 'mandatory reset Return PowerDown: ' should return the motor to fully reclined position and than switch off If nowpos <= MinPos Then Sollpos = MinPos HPWM 2, 0, PWMFreq ' motor off Set CW_Relay Clear CCW_Relay Clear YellowLed Clear GreenLed Set BlueLed Else Sollpos = MinPos ' correct in the main loop EndIf ' reset controllers! CC21 = CC21_default CC22 = CC22_default ' must be fully reclined CC23 = CC23_default MinSpeed = CC21 MaxSpeed = CC23 >> 1 Clear CC67 ' for automove Clear pitch Clear automove Return Vels_Lookup: ' velo lookup scale for Laukhuff solenoid. Logarithmic Vels[1]= 1160 ' duur= .0116 Vels[2]= 1190 ' duur= .0119 Vels[3]= 1206 ' duur= .01206 Vels[4]= 1222 ' duur= .01222 Vels[5]= 1238 ' duur= .01238 Vels[6]= 1254 ' duur= .01254 Vels[7]= 1270 ' duur= .0127 Vels[8]= 1287 ' duur= .01287 Vels[9]= 1303 ' duur= .01303 Vels[10]= 1320 ' duur= .0132 Vels[11]= 1337 ' duur= .01337 Vels[12]= 1355 ' duur= .01355 Vels[13]= 1373 ' duur= .01373 Vels[14]= 1390 ' duur= .0139 Vels[15]= 1408 ' duur= .01408 Vels[16]= 1427 ' duur= .01427 Vels[17]= 1445 ' duur= .01445 Vels[18]= 1464 ' duur= .01464 Vels[19]= 1483 ' duur= .01483 Vels[20]= 1503 ' duur= .01503 Vels[21]= 1522 ' duur= .01522 Vels[22]= 1542 ' duur= .01542 Vels[23]= 1562 ' duur= .01562 Vels[24]= 1582 ' duur= .01582 Vels[25]= 1603 ' duur= .01603 Vels[26]= 1624 ' duur= .01624 Vels[27]= 1645 ' duur= .01645 Vels[28]= 1667 ' duur= .01667 Vels[29]= 1688 ' duur= .01688 Vels[30]= 1710 ' duur= .0171 Vels[31]= 1732 ' duur= .01732 Vels[32]= 1755 ' duur= .01755 Vels[33]= 1778 ' duur= .01778 Vels[34]= 1801 ' duur= .01801 Vels[35]= 1825 ' duur= .01825 Vels[36]= 1848 ' duur= .01848 Vels[37]= 1872 ' duur= .01872 Vels[38]= 1897 ' duur= .01897 Vels[39]= 1921 ' duur= .01921 Vels[40]= 1946 ' duur= .01946 Vels[41]= 1972 ' duur= .01972 Vels[42]= 1997 ' duur= .01997 Vels[43]= 2023 ' duur= .02023 Vels[44]= 2050 ' duur= .0205 Vels[45]= 2077 ' duur= .02077 Vels[46]= 2104 ' duur= .02104 Vels[47]= 2131 ' duur= .02131 Vels[48]= 2159 ' duur= .02159 Vels[49]= 2187 ' duur= .02187 Vels[50]= 2215 ' duur= .02215 Vels[51]= 2244 ' duur= .02244 Vels[52]= 2273 ' duur= .02273 Vels[53]= 2303 ' duur= .02303 Vels[54]= 2333 ' duur= .02333 Vels[55]= 2363 ' duur= .02363 Vels[56]= 2394 ' duur= .02394 Vels[57]= 2425 ' duur= .02425 Vels[58]= 2457 ' duur= .02457 Vels[59]= 2489 ' duur= .02489 Vels[60]= 2521 ' duur= .02521 Vels[61]= 2554 ' duur= .02554 Vels[62]= 2587 ' duur= .02587 Vels[63]= 2621 ' duur= .02621 Vels[64]= 2655 ' duur= .02655 Vels[65]= 2690 ' duur= .0269 Vels[66]= 2725 ' duur= .02725 Vels[67]= 2760 ' duur= .0276 Vels[68]= 2796 ' duur= .02796 Vels[69]= 2833 ' duur= .02833 Vels[70]= 2870 ' duur= .0287 Vels[71]= 2907 ' duur= .02907 Vels[72]= 2945 ' duur= .02945 Vels[73]= 2983 ' duur= .02983 Vels[74]= 3022 ' duur= .03022 Vels[75]= 3061 ' duur= .03061 Vels[76]= 3101 ' duur= .03101 Vels[77]= 3142 ' duur= .03142 Vels[78]= 3183 ' duur= .03183 Vels[79]= 3224 ' duur= .03224 Vels[80]= 3266 ' duur= .03266 Vels[81]= 3309 ' duur= .03309 Vels[82]= 3352 ' duur= .03352 Vels[83]= 3395 ' duur= .03395 Vels[84]= 3440 ' duur= .0344 Vels[85]= 3484 ' duur= .03484 Vels[86]= 3530 ' duur= .0353 Vels[87]= 3576 ' duur= .03576 Vels[88]= 3622 ' duur= .03622 Vels[89]= 3669 ' duur= .03669 Vels[90]= 3717 ' duur= .03717 Vels[91]= 3766 ' duur= .03766 Vels[92]= 3815 ' duur= .03815 Vels[93]= 3864 ' duur= .03864 Vels[94]= 3915 ' duur= .03915 Vels[95]= 3966 ' duur= .03966 Vels[96]= 4017 ' duur= .04017 Vels[97]= 4070 ' duur= .0407 Vels[98]= 4123 ' duur= .04123 Vels[99]= 4176 ' duur= .04176 Vels[100]= 4231 ' duur= .04231 Vels[101]= 4286 ' duur= .04286 Vels[102]= 4342 ' duur= .04342 Vels[103]= 4398 ' duur= .04398 Vels[104]= 4456 ' duur= .04456 Vels[105]= 4514 ' duur= .04514 Vels[106]= 4572 ' duur= .04572 Vels[107]= 4632 ' duur= .04632 Vels[108]= 4692 ' duur= .04692 Vels[109]= 4753 ' duur= .04753 Vels[110]= 4815 ' duur= .04815 Vels[111]= 4878 ' duur= .04878 Vels[112]= 4941 ' duur= .04941 Vels[113]= 5006 ' duur= .05006 Vels[114]= 5071 ' duur= .05071 Vels[115]= 5137 ' duur= .05137 Vels[116]= 5204 ' duur= .05204 Vels[117]= 5272 ' duur= .05272 Vels[118]= 5340 ' duur= .0534 Vels[119]= 5410 ' duur= .0541 Vels[120]= 5480 ' duur= .0548 Vels[121]= 5552 ' duur= .05552 Vels[122]= 5624 ' duur= .05624 Vels[123]= 5697 ' duur= .05697 Vels[124]= 5772 ' duur= .05772 Vels[125]= 5847 ' duur= .05847 Vels[126]= 5923 ' duur= .05923 Vels[127]= 6000 ' duur= .06 Return '[EOF]