'{$STAMP BS2} '************************************************************************* '* <4 Phase stepping motor controller> * '* by * '* Dr.Godfried-Willem RAES * '* Stamp2 Hardware version 1.0 * '* Filename : * '* Code version 1.4 * '************************************************************************* ' 30.07.2002: starting from vibi stepper code, this prg. developped. ' 2-127 runs clockwize - pitch down ' 128-255 runs counterclockwize - pitch up ' 10.09.2002: Stepper data: Type: HY200 3424 470A8 - A436820R1 ' 4 phases - 200 steps per revolution ' Single coil resistance: 0.425 Ohm ' Single coil inductance: 1.2mH ' Series resistors in phase windings: 2.2 Ohms ' Operating voltage: 12V dc ' Version with 8-step mode saved for backup as FlexStepper_V11.bs2 ' 4-step mode kept, since it leads to the highest torque. ' 11.09.2002: Now reception of 1 on the port, will switch flex completely off ' If the motor is running, the 1 has to stay on the port for at least the ' duration of a half revolution of the motor axis. ' To switch Flex back ON you have to resend the pin code (241) ' to be done: add code for sensors at end of trajects. ' 12.09.2002: Note that the stepper board houses two BS2 controllers. ' One controller for the Pi-blade stepper, one for the e-blade stepper ' 13.09.2002: Changed: Pulsout 10 and pulsout 11 replaced with pulsout 9 (NC) ' now we can use pins 10 and 11 for microswitch sensors ' 15.09.2002: We could have an almost identical program for the bowing motors. ' However these should run faster and never slow! ' 26.12.2002: Work on Flex restarted. ' 27.12.2002: code download in stepperstamps. ' 28.12.2002: test and debugging session. Now motor stops at end positions, but stays activated. ' Changes: now it should just stop when the endposition is reached, and disregard any ' further commands in the same direction of rotation. ' 29.12.2002: Now should stop when end position is reached and further commands in the same direction are given. ' Code for both blade BS2's is identical, although only the Pi-motor uses the pincode for ' switching on the relais. ' DECLARATIONS: Centrobyte VAR byte ' incoming byte, from demux board Period VAR word Pincode VAR byte Status VAR byte MaxPos VAR bit ' thread at longest position MinPos VAR bit ' thread as shortest position MaxButton VAR byte ' workspace for button instruction MinButton VAR byte ' workspace for button instruction ' INITIALISATION: ' parallel input bits 0 to 7 ' make this byte an input: DIRL = 0 ' pins 10 and 11 can be used for end sensors. ' pin 8 used as steering bit to switch on the 48V power supply for the solenoids ' after reception of the correct pincode on the datainput bus. ' pin 9 is used as pulsout DIR8 = 1 ' output pin DIR9 = 1 ' output pin DIR10 = 0 ' input pin ' maxpos DIR11 = 0 ' input pin ' minpos 'DIR12 = 1 :' phase 1 output 'DIR13 = 1 :' phase 2 output 'DIR14 = 1 :' phase 3 output 'DIR15 = 1 :' phase 4 output ' these pins steer the gates of 4 logic level 12 Amps power mosfets. ' type IRL620 DIRD = $F OUTD = 0 ' make outputs low. LOW 8 ' keybit ' Initialisation of variables: Pincode = 241 ' = &HF1, second largest 8 bit prime number (Vibi uses 251) Status = 0 ' 0 means motor running clockwize - blades go flat, pitch goes downwards ' at the end position, switch connected to D10 (pin 15) opens ' 1 means motor running counterclockwize - blades bend, pitch goes upwards. ' at the end position, switch connected to D11 (pin 16) opens Maxbutton = 0 MinButton = 0 ' Initialisation code:*************************************************** ' here we wait for reception of PINCODE, before we start. ' Reception of PINCODE will switch 48V power supply on ' for solenoids and steppers. DEBUG "Flex stepping motor controller V1.4 ", cr DEBUG "by dr.Godfried-Willem Raes, 2002", cr Init: Centrobyte = INL IF Centrobyte <> PINCODE THEN Init HIGH 8 ' should switch on motor power supply ' this pin should have a pull down resistor! ' now we should avoid that the motors start running with the value of the pincode... OUTD = 0 SkipPincode: Centrobyte = INL IF Centrobyte = Pincode THEN SkipPinCode 'DEBUG "Ander byte ontvangen...", cr 'DEBUG DEC Centrobyte ' START OF RUN-CODE:***************************************************** Begin: Centrobyte = INL BRANCH Centrobyte,[MotorOff,FlexOff] ' centrobyte 0 switches motors off, 1 switches Flex off and ' requires the user to resend the pincode. Status = IN7 ' 0 or 1 - direction of rotation Centrobyte = Centrobyte & 127 ' mask highest bit ! Period = 128 - Centrobyte ' 126 - 1 ' minimale praktische waarde voor Period is ... Period = Period << 5 ' = * 32 ' 1 unit of period for BS2-stamp is 2 microseconds Period = Period + 468 ' nu is de minimumwaarde = 468+32 = 500, of 1.000ms ' dus wordt het snelste tempo: 200 * 1.000ms = 200 ms / omwenteling ' of = 5 Hz ' en de traagste snelheid: ' 4064 + 200 = 4264 * 2 = 8.528 ms ' dus wordt het traagste tempo: 200 * 8.528ms = 1.7056s per omwenteling ' of, 0.586Hz IF IN10 THEN EndPosMax 'gestrekte eindpositie is bereikt. Alleen AntiClock4Step is nu mogelijk. IF IN11 THEN EndPosMin 'opgespannen eindpositie is bereikt. Alleen Clock4Step is nu mogelijk. MaxPos = 0 MinPos = 0 BRANCH Status, [Clock4Step, AntiClock4Step] Clock4step: ' Strekken van de bladveer, pitch goes down. ' 1 4 x 25 steps = 100 steps, 1/2 revolution PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 2 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 3 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 4 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 5 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 6 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 7 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 8 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 9 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 10 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 11 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 12 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 13 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 14 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 15 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 16 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 17 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 18 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 19 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 20 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 21 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 22 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 23 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 24 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 ' 25 PulsOut 9, Period OUTD = 3 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD =9 GOTO BEGIN AntiClock4step: ' Spannen van de bladveer - Pitch goes up. ' 1 ' 25 x 4 steps = 100 steps, or half revolution PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 2 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 3 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 4 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 5 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 6 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 7 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 8 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 9 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 10 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 11 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 12 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 13 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 14 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 15 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 16 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 17 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 18 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 19 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 20 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 21 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 22 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 23 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 24 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 ' 25 PulsOut 9, Period OUTD = 9 PulsOut 9, Period OUTD = 12 PulsOut 9, Period OUTD = 6 PulsOut 9, Period OUTD =3 GOTO BEGIN ' end 4-step counterclockwize MotorOff: OUTD = 0 ' = LOW 12, LOW 13, LOW 14, LOW 15 GOTO BEGIN FlexOff: OUTD = 0 ' motor off LOW 8 ' switches pin-code relais off GOTO INIT EndPosMax: Maxpos = 1 Minpos = 0 ' IF Status = 0 THEN MotorOff ' wanneer het kommando nog steeds om strekken vraagt, schakelen we de motor uit. ' ' indien het kommando echter de draairichting omkeert, door status 1 te maken, dan voeren we ' ' dit uit. Noteer dat het meer dan een halve asomwenteling vraagt eer de microswitch gedesactiveerd ' ' wordt. ' 'Button 10,1,255,0,MaxButton,1, AntiClock4Step ' ' pin 10, detect 1 state, debounce but no repeat, no delay, workspace byte, detect pressed button, label ' ' buttoncode did'nt do it very well, so we simply replaced it by: ' IF Status = 1 THEN AntiClock4Step ' optimized coding: BRANCH Status, [MotorOff, AntiClock4Step] GOTO BEGIN EndPosMin: MinPos = 1 MaxPos = 0 ' IF Status = 1 Then MotorOff ' 'Button 11,1,255,0,MinButton,1, Clock4Step ' IF Status = 0 THEN Clock4Step ' optimized coding: BRANCH Status, [Clock4Step, MotorOff] GOTO BEGIN STOP