BASIC programs used by Santa Clara students
to position the RAMACThe following is from a CDROM found in the RAMAC area of CHM Saturday July 1, 2006
I don't know where it came from, and maybe I shouldn't have borrowed it. However, with the purpose of "keeping us informed", here goes :-| I presume it is the source code used by the Santa Clara students to drive the RAMAC servo system
Start of tirade :-((
In part, I am including this BASIC code because uninformed people often laugh and sniff at BASIC as being "primitive".OK, it might not be the best thing for writing the UNIX operating system, but for many practical purposes it is much simpler, less error prone, and easier to understand. You can write tough code in any language - for fun you might wish to observe annual contest to write "Obfuscated C" - And then there is C++ :-(( End of tirade ;-)) The code on the disk seems organized in the following way
"Main Control" Directory Main.bas, / DiskPos.bas, / ArmPos.bas, / Servo.bas / "Disk Position" Directory DiskPos.bas, / Servo.bas / "Arm Position" Directory Position.bas, / Servo.bas / "Head Trial" Directory Heads.bas /
There were other files of type .bxb, .bxm, .bxp, .mpx, .mpp, .prfwhich I did not include - maybe make files, .h type files, and object files?
Main Control - Main.bas
Option Explicit 'Constant Declarations Private DISKARRAY(1 to 50) as New PersistentInteger Public Const DiskWiper as Byte = 13 Public Const ArmWiper as Byte = 14 Public Const TrackOdd as Byte = 7 Public Const TrackEven as Byte = 5 Public Const DiskOut as Byte = 6 Public Const DiskIn as Byte = 9 Public Const AirHead as Byte = 8 Public Const InDown as Byte = 15 Public Const OutUp as Byte = 16 Public Const KpA as Integer = 8 Public Const KvA as Integer = 15 Public Const KpD as Single = 30.0 Public Const KvD as Single = 40.0 Public Const ArmHome as Integer = 15 Public Const AntiGrav as Integer = 3300 'Integer Public Const max as integer = 4 'Variable Declarations Public ArmDest as Integer Public ArmSave as Integer Public ArmErr as Integer Public ArmPos as Integer Public ArmLast as Integer Public ArmVel as Integer Public Detent as Byte Public DSave as Byte Public Done as Boolean Public PWMVal as Integer Public InRate as Integer Public OutRate as Integer Public DiskPos as Integer Public DiskLast as Integer Public DiskVel as Integer Public DiskDest as Integer Public DiskErr as Integer Public DiskInt as Integer Public J as Integer 'Public DIntArr(1 to 15) as Integer Public K as Integer 'Stacks and Serial Buffers Dim ServoStack(1 To 40) As Byte Public Icom1(1 to 11) As Byte Public Ocom1(1 to 11) As Byte Public Sub Main() 'Call PersMem() ' Start servo multitasking, start serial communication CallTask "Servo", ServoStack Call OpenQueue(ICom1, 13) Call OpenQueue(OCom1, 13) Call OpenCom(1,19200,ICom1,OCom1) do ' Get disk and track information Call GetInfo() ' Go to track home Call GoHome() ' Go to correct disk Call PDdisk() ' Go to correct track Call PDArm() ' Announce finished Call AllDone() loop End Sub '-------------------------------------------------------------------------- Sub GetInfo() dim A as Byte dim B as Byte Call ClearQueue(ICom1) debug.print "What disk would you "; Debug.Print "like?(01 to 50)" Call GetQueue(ICom1, A, 1) Call GetQueue(ICom1, B, 1) A = A - 48 B = B - 48 DiskDest = Cint(10*A + B) If DiskDest > 50 Then DiskDest = 50 End If ' Calculates exact disk ADC Value from Persistent Memory DiskDest = DISKARRAY(DiskDest) Call ClearQueue(ICom1) debug.print "What track would you "; Debug.Print "like?(00 to 99)" Call GetQueue(ICom1, A, 1) Call GetQueue(ICom1, B, 1) A = A - 48 B = B - 48 ArmDest = Cint(10*A + B) If (ArmDest MOD 2) = 0 Then Detent = TrackEven Else Detent = TrackOdd End If ' Calculates approx track ADC value based on rude curve fit ' needs lots of work ArmDest = 174 + Cint(8.23*Csng(ArmDest)) 'Debug.Print "Track: "; CStr(ArmDest) End Sub '-------------------------------------------------------------------------- Sub AllDone() ArmPos = GetADC(ArmWiper) DiskPos = GetADC(DiskWiper) Debug.Print " Disk Desired: "; CStr(DiskDest) Debug.Print " Disk: "; CStr(DiskPos) Debug.Print "Track Desired: "; CStr(ArmDest) Debug.Print " Track: "; CStr(ArmPos) End Sub '-------------------------------------------------------------------------- sub PersMem() ''WARNING: this sub writes to eeprom, only use the first time ''the prog is run DISKARRAY(1) = 1022 DISKARRAY(2) = 1008 DISKARRAY(3) = 987 DISKARRAY(4) = 966 DISKARRAY(5) = 945 DISKARRAY(6) = 924 DISKARRAY(7) = 904 DISKARRAY(8) = 883 DISKARRAY(9) = 862 DISKARRAY(10) = 840 DISKARRAY(11) = 820 DISKARRAY(12) = 800 DISKARRAY(13) = 779 DISKARRAY(14) = 759 DISKARRAY(15) = 739 DISKARRAY(16) = 718 DISKARRAY(17) = 698 DISKARRAY(18) = 679 DISKARRAY(19) = 660 DISKARRAY(20) = 642 DISKARRAY(21) = 623 DISKARRAY(22) = 604 DISKARRAY(23) = 584 DISKARRAY(24) = 563 DISKARRAY(25) = 543 DISKARRAY(26) = 523 DISKARRAY(27) = 505 DISKARRAY(28) = 486 DISKARRAY(29) = 466 DISKARRAY(30) = 447 DISKARRAY(31) = 426 DISKARRAY(32) = 407 DISKARRAY(33) = 388 DISKARRAY(34) = 369 DISKARRAY(35) = 348 DISKARRAY(36) = 328 DISKARRAY(37) = 308 DISKARRAY(38) = 288 DISKARRAY(39) = 267 DISKARRAY(40) = 247 DISKARRAY(41) = 225 DISKARRAY(42) = 205 DISKARRAY(43) = 184 DISKARRAY(44) = 164 DISKARRAY(45) = 143 DISKARRAY(46) = 122 DISKARRAY(47) = 100 DISKARRAY(48) = 79 DISKARRAY(49) = 52 DISKARRAY(50) = 24 End Sub '--------------------------------------------------------------------------
Main Control - DiskPos.bas
Sub PDDisk() Done = False J = 1 K = 0 DiskErr = 0 OutRate = AntiGrav Call Delay(0.1) Call Putpin(Diskout,1) Call Putpin(Diskin,0) 'Main Control Loop Do ' Call PulseOut(17, 1000, 1) ' Calculate Position and Velocity Call DiskState() ' PD Control Loop PWMVal = Cint(Clng(Kpd)*Clng(DiskErr) - Clng(Kvd*Csng(DiskVel))) + AntiGrav '+ KiD*Csng(DiskInt) Call GoDisk() If ABS(DiskVel)<=5 then If ABS(DiskErr) <= 4 Then K = K + 1 Else K = 0 End If End If If K = 10 Then Done = True End If Loop While (Done = False) Call LockDisk() ' DiskPos = GetADC(DiskWiper) ' Debug.Print " At Disk: "; Cstr(DiskPos) End Sub '-------------------------------------------------------------------------- Sub DiskState() 'Calculates Position DiskPos = GetADC(DiskWiper) 'Calculates Error DiskErr = DiskDest - DiskPos 'Calculates Velocity DiskVel = DiskPos - DiskLast DiskLast = DiskPos 'Debug.Print "ArmErr = "; Cstr(DiskErr) 'Debug.Print "ArmVel = "; Cstr(DiskVel) 'Debug.Print "ArmInt = "; Cstr(DiskInt) End Sub '--------------------------------------------------------------------------Main Control - ArmPos.bas
Sub PDArm() ' Initalize looping conditions Done = False J = 1 K = 0 ' Unlocks Detents Call PutPin(TrackOdd,0) Call PutPin(TrackEven,0) Do ' Call PulseOut(17, 1000, 1) ' Calculate Position and Velocity Call ArmState() ' PD Control Algorithm PWMVal = KpA*ArmErr - KvA*ArmVel ' calculates necessary direction If Abs(PWMVal) = PWMVal then 'positive Call Goin() Else PWMVal = Abs(PWMVal) Call GoOut() End If If ABS(ArmVel)<=5 Then ' Enters this If-Then only when going slowly If ABS(ArmErr) <= 2 Then K = K + 1 Else K = 0 End If End If If K = 5 Then Done = True Call GoStop() Call PutPin(Detent,1) End If ' Done = StatusQueue(ICom1) Loop While (Done = False) ArmPos = GetADC(ArmWiper) 'Debug.Print "At Track: "; Cstr(ArmPos) End Sub '-------------------------------------------------------------------------- Sub ArmState() ' Calculates Position ArmPos = GetADC(ArmWiper) 'Debug.print "ArmPos: "; Cstr(ArmPos) ' Calculates Error ArmErr = ArmDest - ArmPos ' Calculates Velocity ArmVel = ArmPos - ArmLast ArmLast = ArmPos 'Debug.Print "ArmPos = "; Cstr(ArmPos) 'Debug.Print "ArmVel = "; Cstr(ArmVel) End Sub '--------------------------------------------------------------------------Main Control - Servo.bas
Sub Servo() ' These variables should never exceed 19000. InRate = 1 OutRate = 1 Do ' Generate a high-going pulse on In/Up Clutch. Call PulseOut(InDown, InRate, 1) ' This is to produce a pulse rate of about 50 Hz. Call Delay(0.005) ' Call PulseOut(OutUp, OutRate, 1) ' This is to produce a pulse rate of about 50 Hz. Call Delay(0.005) Loop End Sub '-------------------------------------------------------------------------- 'Track Motion Only Sub GoIn () OutRate = 1 If PWMVal < 1000 then InRate = 1000 ElseIf PWMVal > 1400 then InRate = 1400 Else InRate = PWMVal End If End Sub '-------------------------------------------------------------------------- 'Track Motion Only Sub GoOut () InRate = 1 If PWMVal < 1100 then OutRate = 1100 ElseIf PWMVal > 1400 then OutRate= 1400 Else OutRate = PWMVal End If End Sub '-------------------------------------------------------------------------- Sub GoStop () InRate = 1 OutRate = 1 End Sub '-------------------------------------------------------------------------- 'Disk Motion Only Sub GoDisk() InRate = 1 If PWMVal <2500 then OutRate = 2500 ElseIf PWMVal > 6000 Then Outrate = 6000 Else OutRate = PWMVal End If End Sub '-------------------------------------------------------------------------- Sub LockDisk() call Putpin(Diskin,1) call Putpin(Diskout,0) Call Delay(0.2) Outrate = 1 InRate = 1 ' Turns on GreenLED Call PutPin(26,0) End Sub '-------------------------------------------------------------------------- Sub GoHome() ' Saves destination track while going home ArmSave = ArmDest ArmDest = ArmHome DSave = Detent Detent = 26 Call PDArm() Detent = DSave ArmDest = ArmSave End SubArm Position - Position.bas
Option Explicit 'Constant Declarations Public Const DiskWiper as Byte = 13 Public Const ArmWiper as Byte = 14 Public Const TrackOdd as Byte = 7 Public Const TrackEven as Byte = 5 Public Const DiskOut as Byte = 6 Public Const DiskIn as Byte = 9 Public Const AirHead as Byte = 8 Public Const InUp as Byte = 15 Public Const OutDown as Byte = 16 Public Const Kp as Integer = 3 Public Const Kv as Integer = 6 Public Const Clmin as Integer = 900 Public Const Clmax as Integer = 1200 'Variable Declarations Dim ArmDest as Integer Public ArmErr as Integer Public ArmPos as Integer Public ArmLast as Integer Public ArmVel as Integer Public Direction as Boolean ' if true then going in/up Public PWMVal as Integer Public InRate as Integer Public OutRate as Integer Dim ServoStack(1 To 40) As Byte Public Sub Main() CallTask "Servo", ServoStack ArmDest = 50 'Converts track number to 10 bit representation ArmDest = 550 Debug.Print "Track: 50" Debug.print " Value: "; Cstr(ArmDest) do 'Calculate Position and Velocity Call State() 'calculates offset ArmErr = ArmDest - ArmPos 'if abs(armerr) = armerr then ' armerr = armerr + fudge 'else ' armerr = armerr - fudge 'end if 'PD Control Algorithm PWMVal = Kp*(ArmErr ) - Kv*ArmVel 'calculates necessary direction If Abs(PWMVal) = PWMVal then 'positive 'Direction = True Call Goin() Else 'Direction = False PWMVal = Abs(PWMVal) Call GoOut() End If 'call Goin() loop End Sub '-------------------------------------------------------------------------- Sub State() dim I as integer ArmPos = 0 for I = 0 to 4 ArmPos = ArmPos + GetADC(ArmWiper) next ArmPos=ArmPos\5 'Calculates Velocity ArmVel = ArmPos - ArmLast ArmLast = ArmPos Debug.Print "ArmPos = "; Cstr(ArmPos) 'Debug.Print "ArmVel = "; Cstr(ArmVel) End Sub '--------------------------------------------------------------------------Arm Position - Servo.bas
Sub Servo() ' These Variables should never exceed 0.0019. InRate = 1 OutRate = 1 'Call Delay(1.0) Do ' Generate a high-going pulse on In/Up Clutch. Call PulseOut(InUp, InRate, 1) ' This is to produce a pulse rate of about 50 Hz. Call Delay(0.005) ' Call PulseOut(OutDown, OutRate, 1) ' This is to produce a pulse rate of about 50 Hz. Call Delay(0.005) Loop End Sub '-------------------------------------------------------------------------- Sub GoIn () OutRate = 1 'inrate=750 If PWMVal < (Clmin+40) then InRate = Clmin+40 ElseIf PWMVal > Clmax then InRate = Clmax Else InRate = PWMVal End If ' Debug.Print "Going in"; Cstr(InRate) End Sub '-------------------------------------------------------------------------- Sub GoOut () InRate = 1 If PWMVal < ClMin then OutRate = ClMin ElseIf PWMVal > Clmax then OutRate=Clmax Else OutRate = PWMVal End If ' Debug.Print "Going out"; Cstr(outRate) 'OutRate = 3000 'Call Delay(0.015) End Sub '-------------------------------------------------------------------------- Sub GoStop () InRate = 1 OutRate = 1 End SubDisk Position - DiskPos.bas
Option Explicit 'Constant Declarations Public Const DiskWiper as Byte = 13 Public Const ArmWiper as Byte = 14 Public Const TrackOdd as Byte = 7 Public Const TrackEven as Byte = 5 Public Const DiskOut as Byte = 6 Public Const DiskIn as Byte = 9 Public Const AirHead as Byte = 8 Public Const InUp as Byte = 15 Public Const OutDown as Byte = 16 Public Const Kp as Integer = 3 Public Const Kv as Integer = 7 Public Const Clmin as Integer = 900 Public Const Clmax as Integer = 1200 'Constants for down motion 'Public Const KpDd as Single = 1.0 'Public Const KiDd as Single = 0.0 'Public Const KvDd as Single = 0.0 'Constants for up motion Public Const KpDu as Single = 19.0 'Public Const KiDu as Single = 0.0 Public Const KvDu as Single = 39.0 Public Const AntiGrav as Integer = 3350 'Integer Public Const max as integer = 4 'Variable Declarations Dim ArmDest as Integer Public ArmErr as Integer Public ArmPos as Integer Public ArmLast as Integer Public ArmVel as Integer Public Direction as Boolean ' if true then going in/up Public Done as Boolean Public PWMVal as Integer Public InRate as Integer Public OutRate as Integer Public DiskPos as Integer Public DiskLast as Integer Public DiskVel as Integer Public DiskDest as Integer Public DiskErr as Integer Public DiskInt as Integer Public J as Integer Dim KpD as Single Dim KiD as Single Dim KvD as Single 'Dim max as Integer Public DIntArr(1 to 15) as Integer Dim K as Integer Dim ServoStack(1 To 40) As Byte Sub PDdisk() CallTask "Servo", ServoStack DiskDest = 1011 DIntArr(1) = 0 DIntArr(2) = 0 DIntArr(3) = 0 DIntArr(4) = 0 DIntArr(5) = 0 DIntArr(6) = 0 DIntArr(7) = 0 DIntArr(8) = 0 Done = False J = 1 K = 0 DiskErr = 0 'Max = Maxint OutRate = AntiGrav call Putpin(Diskout,1) call Putpin(Diskin,0) Kpd = Kpdu ' KiD = KiDu KvD = KvDu 'Main Control Loop do ' Call PulseOut(17, 1000, 1) Call DiskState() 'Decision on direction: which PID gains to use 'DiskErr = DiskDest - DiskPos 'If Abs(DiskErr) = DiskErr Then ' Debug.Print "Going Up" ' Kpd = Kpdu ' KiD = KiDu ' KvD = KvDu 'Else ' Debug.Print "Going Down" ' Kpd = Kpdd ' KiD = KiDd ' KvD = KvDd 'End If 'If Abs(DiskErr) < 30 Then ' Kpd = 20.0 ' KiD = 20.0 ' KvD = 4.0 'end if 'PID Control Loop PWMVal = Cint(Kpd*Csng(DiskErr) + KiD*Csng(DiskInt) - Kvd*Csng(DiskVel)) + AntiGrav Debug.Print "PWM = "; Cstr(PWMVal) If ABS(DiskErr) <= 2 Then ' Done = True K = K + 1 Debug.Print " K+1 DiskErr = "; Cstr(DiskErr); Cstr(DiskPos) Else K = 0 End If If K = 3 Then Done = True End If Call GoDisk() loop While Done = False Call LockDisk() Debug.Print "All Done!" End Sub '-------------------------------------------------------------------------- Sub DiskState() dim I as integer 'Calculates Position 'DiskPos = GetADC(DiskWiper) 'DiskPos = GetADC(DiskWiper) DiskPos = 0 for I = 0 to 4 DiskPos = DiskPos + GetADC(DiskWiper) next DiskPos=DiskPos\5 Debug.Print " ArmPos = "; Cstr(DiskPos) 'Calculates Error DiskErr = DiskDest - DiskPos 'Calculates Velocity DiskVel = DiskPos - DiskLast DiskLast = DiskPos 'Call Integrate() 'Debug.Print "ArmErr = "; Cstr(DiskErr) 'Debug.Print "ArmVel = "; Cstr(DiskVel) 'Debug.Print "ArmInt = "; Cstr(DiskInt) End Sub '-------------------------------------------------------------------------- Sub Integrate() Dim I as Integer 'Calculates Integral error 'If Abs(DiskErr) > 800 Then ' DiskInt = 0 'Else DIntArr(J) = DiskErr DiskInt = 0 For I = 0 to (max-1) DiskInt = DiskInt + DIntArr(I) Next DiskInt = DiskInt\max 'If DiskInt < -15 then ' DiskInt = -15 'End If J = J + 1 If J > max Then J = 1 End If 'End If End Sub Sub LockDisk() call Putpin(Diskin,1) call Putpin(Diskout,0) Call Delay(0.1) Outrate = 1 InRate = 1 Call PutPin(26,0) End SubDisk Position - Servo.bas
Sub Servo() ' These Variables should never exceed 0.0019. InRate = 1 OutRate = 1 'Call Delay(1.0) Do ' Generate a high-going pulse on In/Up Clutch. Call PulseOut(InUp, InRate, 1) ' This is to produce a pulse rate of about 50 Hz. Call Delay(0.005) ' Call PulseOut(OutDown, OutRate, 1) ' This is to produce a pulse rate of about 50 Hz. Call Delay(0.005) Loop End Sub '-------------------------------------------------------------------------- Sub GoIn () OutRate = 1 'inrate=750 If PWMVal < (Clmin+40) then InRate = Clmin+40 ElseIf PWMVal > Clmax then InRate = Clmax Else InRate = PWMVal End If ' Debug.Print "Going in "; Cstr(InRate) End Sub '-------------------------------------------------------------------------- Sub GoOut () InRate = 1 If PWMVal < ClMin then OutRate = ClMin ElseIf PWMVal > Clmax then OutRate=Clmax Else OutRate = PWMVal End If ' Debug.Print "Going out"; Cstr(outRate) 'OutRate = 3000 'Call Delay(0.015) End Sub '-------------------------------------------------------------------------- Sub GoStop () InRate = 1 OutRate = 1 End Sub '-------------------------------------------------------------------------- Sub GoDisk() InRate = 1 If PWMVal <2200 then OutRate = 2200 'ElseIf PWMVal > 10000 Then ' Outrate = 10000 Else OutRate = PWMVal End If ' Debug.Print "Going "; Cstr(OutRate) End Sub '-------------------------------------------------------------------------- Sub GoDownDisk() InRate = 1 If PWMVal <2200 then OutRate = 2200 'ElseIf PWMVal > 10000 Then ' Outrate = 10000 Else OutRate = PWMVal End If Debug.Print "Going down "; Cstr(OutRate) End SubHead Trial - Heads.bas
Option Explicit Public Const DiskWiper as Byte = 13 Public Const ArmWiper as Byte = 14 Public Const TrackOdd as Byte = 7 Public Const TrackEven as Byte = 5 Public Const DiskOut as Byte = 6 Public Const DiskIn as Byte = 9 Public Const AirHead as Byte = 8 Public Const InDown as Byte = 15 Public Icom1(1 to 11) As Byte Public Ocom1(1 to 11) As Byte public armpos as integer Public Sub Main() Call OpenQueue(ICom1, 13) Call OpenQueue(OCom1, 13) Call OpenCom(1,19200,ICom1,OCom1) Do Call Putpin(TrackOdd,0) Call PutPin(AirHead,0) Call ClearQueue(ICom1) 'Waits for any key Do Call Delay(0.1) Loop While (StatusQueue(ICom1) = False) 'Call Putpin(TrackOdd,1)'locks the odd track detent, comment this out to prevent track locking Call PutPin(AirHead,1)'puts heads down 1: on, 0: off 'Call DiskWrite()'calls command to write disk, comment out to avoid writing Call ClearQueue(ICom1) 'Waits for any key Do Call Delay(0.1) Loop While (StatusQueue(ICom1) = False) 'Call Putpin(TrackOdd,0) 'Call PutPin(AirHead,0) Loop End Sub Sub DiskWrite() Debug.Print "Writing" Call Delay(0.5) Call PutPin(11,0) Dim I as Integer For I = 1 to 5000 '1.0e-5=10 microseconds Call Pulseout(12, 1.0E-4,1) Call Pulseout(11, 1.0E-4,1) Next Debug.Print "Done" End Sub
If you have comments or suggestions, Send e-mail to Ed Thelen