' -----[ Title ]---------------------------------------------------------------- ' Modified for new Co-Processor board and production Twinkle Toes 8/7/03 ' filename = Toddler CoP Bd Program A.2.BS2 ' Has more steps in Basic Movement Tables - go to center first rather than ' tilt right to left - has calculated delays and different tilt rates ' Taken from Toddler Programs A.1 and 9.2 Memsic Walker.bs2 ' This program has real time servo programming and sensor integration. ' Toddler now wanders in a straight line until it sees or hits something. ' by H. W. Lewis (www.bluebelldesign.com) ' {$STAMP BS2} ' {$PBASIC 2.5} ' DON'T FORGET TO USE THE RESET SWITCH WHEN POWERING UP. ' Servo 0 = Tilt, Servo channel 1 = Stride ' The ground (or black) wire goes closest to PCB and ' the control (or white) wire goes furthest from PCB ' see I/O Definitions below for other pin connections ' -----[ I/O Definitions ]------------------------------------------------------ To_Co_Proc PIN 9 ' data to BBDI Co-Processor(Sin) Frm_Co_Proc PIN 10 ' data from BBDI Co-Processor(Sout) Timer_Exp PIN 7 ' Timer Expired from BBDI Co-Processor Rst_Co_Proc PIN 8 ' reset to BBDI Co-Processor LeftIRLED PIN 4 ' left IRLED RightIRLED PIN 15 ' right IRLED LeftIRSensor PIN 11 ' left IR sensor input RightIRSensor PIN 14 ' right IR sensor input RightBumper PIN 1 ' Toddler Toes LeftBumper PIN 2 Spkr PIN 13 ' beeper pin CycleIndicator PIN 12 ' shows how fast main loop is going ' - toggles each time through - NOT REALLY NEEDED Yout PIN 5 ' Yout from Memsic 2125 was pin 10 in ' other example programs 'EMPTY PIN 6 ' spare Stamp I/O pin ' -----[ #DEFINES ]------------------------------------------------------------- #DEFINE CalibrateFlatMode = 0 ' 1 will read your value for the Memsic ' sensor when straight up. If set, ' supercedes all other modes. ' 0 = not running #DEFINE SwayMode = 0 ' 1 will tilt back and forth only ' no striding - good for setup ' -----[ Constants ]------------------------------------------------------------ ' Right and left depend on ' how you mounted the Memsic ' sensor. Below assume pins 1 & 6 ' are toward front of Toddler. ' Smaller numbers mean Toddler is tilted ' LEFT RtTilt CON 32 ' Yout change in right direction LftTilt CON 30 ' Yout change in left direction YAxisCenter CON 2508 ' Yout when Bot straight up on flat ' surface - mine 2508 = 5.016 ms YAxisRtLimit CON YAxisCenter+RtTilt ' Yout right limit YAxisLfLimit CON YAxisCenter-LftTilt ' Yout left limit ServOvrLeft CON 140 ' servo is toward left side ServOvrRight CON 115 ' servo is toward right side ' small numbers mean servo is trying ' to tilt RIGHT escVal CON 3 ' tiltCounter filter for tilt measurement ' Comments below hold original values so, when tuning, you can get back ' to where you started. These values are for my Toddler, ' yours are probably different. Tilt limits can be found by using Sway and ' CalibrateFlat routines. Comment them out in Main when you are done tuning. TiltStep1 CON 8 ' TiltServo step size from center TiltStep2 CON 16 ' TiltServo step size to center ' in units of 1/4 step per 20ms ' so 8 => 2 full servo steps/20ms ' Tilt is set to full span - actual limit on tilt is from Memsic tilt sensor. RightTilt CON 2 ' Tilt limits ' (Toddler was 500 = 1000us) ' CoP = (2*4us)+ 994 = 1002us CenterTilt CON 127 ' (Toddler was 750 = 1500us) ' CoP = (127*4us)+ 994 = 1502us LeftTilt CON 251 ' (Toddler was 1000 = 2000us) ' CoP = (251*4us)+ 994 = 1998us StrideStep CON 20 ' StrideServo step size ' 20 = 5 steps/20ms RightStride CON 76 ' Stride limits ' (Toddler was 650 = 1300us) ' CoP = (76*4us)+ 994 = 1298us CenterStride CON 127 ' (Toddler was 750 = 1500us) ' CoP = (127*4us)+ 994 = 1502us LeftStride CON 176 ' (Toddler was 850 = 1700us) ' CoP = (176*4us)+ 994 = 1698us Baud CON 84 ' 9600 Baud => BS2 = 84, BS2p = 240 TiltServo CON 136 ' use Servo 0 = Tilt ' (136 = WRITE setpoint command) StrideServo CON 137 ' use Servo 1 = Stride TiltServoPos CON 24 ' read tilt servo (0) current position ' in Co-Proc StrideServoPos CON 25 ' read stride servo (1) current position ' in Co-Proc TiltRamp CON 144 ' start value to write servo 0 ramp ' (command = 128 + 16 + ch0 = 144) StrideRamp CON 145 ' write servo 1 ramp ' = 1 more than servo 0 command ' Co-Proc servo channels can also be used as general outputs. ' Here channels 6 AND 7 are used for vision indicator LED drive. ' 6 = Left vision, 7 = Right vision, High out = on. LeftVisionLED_Off CON 246 ' left LED - 0 = Off LeftVisionLED_On CON 254 ' left LED - 1 = On ' executing vision sequence for left side RightVisionLED_Off CON 247 ' right LED - 0 = Off RightVisionLED_On CON 255 ' right LED - 1 = On ' executing vision sequence for right side ctrlLEDs CON %1111 ' define DIRs as constants to prepBumper CON %1001 ' simplify LED/bumper setups bothGreen CON %0110 ' OUTa register sets red/green bothRed CON %1001 ' status on P0-P3 for Twinkle Toes rightRed CON %0101 ' object detection states leftRed CON %1010 bothLEDsOff CON %1001 Fwd CON 0 ' codes to pick movement table PivL CON 1 ' set newMove to one of these PivR CON 2 ' values and New_Movemt will LOOKUP BumpL CON 3 ' the right movement table BumpR CON 4 ' -----[ Variables ]------------------------------------------------------------ delayTime VAR Byte ' # of 20 ms cycles to wait for ' move to complete serDIn VAR delayTime ' variable where Co-Processor ' DATA comes back - reused doTiltFlag VAR Bit ' flag: 1 = need to measure tilt ' flag: 0 = no need to measure tiltingLeftFlag VAR Bit ' flag: 1 = tilting LEFT ' (also tilt servo new value > old) ovrCntrFlag VAR Bit ' flag: 1 = tilt beyond half way doneMoveFlag VAR Bit ' flag: 1 = servos hit new values rightIR_Flag VAR Bit ' flag: 1 = something on right side leftIR_Flag VAR Bit ' flag: 1 = something left side rightBumpFlag VAR Bit ' flag: 1 = bumper hit right side leftBumpFlag VAR Bit ' flag: 1 = bumper hit left side sensors VAR Nib ' lower 2 bits of the sensors var ' used to store IR detector values currentMove VAR Nib newMove VAR Nib tiltCounter VAR Nib ' Counter filters tilt sensor output Mx VAR Word ' index for movement tables MxOffset VAR Byte ' added to Mx for index bMovmnt VAR Byte ' table value for lookup movement currentTilt VAR Byte ' used to be Words currentStride VAR Byte ' - saves variable space newValue VAR Byte newStride VAR Byte Incline VAR Word ' Pulses from Memsic 2125 ' -----[ EEPROM Data ]---------------------------------------------------------- ' ' These are actual values saved in the Basic Movement tables. ' They are coded so the bits can be used rather than a CASE statement. ' This way it saves a lot of code and flags. It will also allow for ' using the upper nibble for something else. TL CON 0 ' use lower nibble for these TC CON 1 TR CON 2 SL CON 4 SC CON 5 SR CON 6 xx CON 255 ' table end code ' ------ Basic Movement Tables ------ ' ' These tables contain Basic Movements consisting of bytes containing ' above Basic Movement Codes to describe movement sequences. ' An xx indicates the end of a list. ' PivotLeft and PivotRight aren't entered at the start of their tables Forward DATA TR, SL, TC, TL, SR, TC, xx ' possible rt bumper hit while SR ' left bumper hit while SL PivotLeft DATA TR, SL, TC, TL, SR, TC ' copy of forward, index into here ' by keeping old MxOffset value from Forward move. DATA SL, TL, SR, TC, xx ' this is the actual pivot move. PivotRight DATA TR, SL, TC, TL, SR, TC ' copy of forward, index into here ' by keeping old MxOffset value from Forward move. DATA TR, SL, TC, SR, xx ' this is the actual pivot move. RBumper DATA SL, TC, TR, SR, TC, SL, TL, SR, TC, xx 'back up 1 step and pivot left ' bumper hit assumes SR (@TL) when hit happened. LBumper DATA SR, TC, TL, SL, TC, SR, TR, SL, TC, TL, SR, TC, xx 'back up 1 step and pivot right ' bumper hit assumes SL (@TR) when hit happened. #IF (SwayMode = 1) #THEN SwayIt DATA TR, TC, TL, TC, xx ' used to sway back and ' forth for tuning #ENDIF ' -----[ Initialize ]----------------------------------------------------------- SEROUT To_Co_Proc, BAUD, [116] 'Sets up serial pin LOW Rst_Co_Proc 'Reset Co-Processor PAUSE 20 HIGH Rst_Co_Proc GOSUB Clr_Vision ' reset vision LEDs and flags DIRS = ctrlLEDs ' setup green LEDs for Forward OUTS = bothGreen tiltCounter = 0 ' reset tilt filter SEROUT To_Co_Proc, Baud, [TiltRamp,TiltStep1] 'write rate to servo 0 SEROUT To_Co_Proc, Baud, [StrideRamp,StrideStep] 'write rate to servo 1 ResetFeet: CurrentTilt = CenterTilt CurrentStride = CenterStride SEROUT To_Co_Proc, Baud, [TiltServo,CurrentTilt] 'Set Servo 0 = Tilt SEROUT To_Co_Proc, Baud, [StrideServo,CurrentStride] 'Set Servo 1 = Stride 'DEBUG "resetCC", CR, lf 'DEBUG "Forward = ", hex Forward, cr 'DEBUG "PivotLeft = ", hex PivotLeft, cr, "PivotRight = ", hex PivotRight, cr 'DEBUG "RBumper = ", hex RBumper, cr, "LBumper = ", hex LBumper, cr doneMoveFlag = 1 ovrCntrFlag = 0 sensors = 0 MxOffset = 0 ' Turn off bumper LEDs if being driven from servo channels 4 & 5 ' - DO NOT DO THIS IF THEY ARE BEING USED AS SERVO CHANNELS! ' The LEDs are not used in this program but may be wired in. SEROUT To_Co_Proc, BAUD, [252] 'output servo ch 4 = 1 -> Co-Processor SEROUT To_Co_Proc, BAUD, [253] 'output servo ch 5 = 1 -> Co-Processor PAUSE 1000 ' wait with the feet straight at startup FREQOUT Spkr, 2000, 3000 ' program start/restart signal rightBumpFlag = 0 leftBumpFlag = 0 currentMove = 15 ' invalid value to assure start ' newMove = Fwd ' for testing single moves - ' newMove = PivL ' comment out GOSUBs to vision ' newMove = PivR ' and bump or the value might be ' newMove = BumpL ' overwritten ' newMove = BumpR bMovmnt = TR ' -----[ Main Code ]------------------------------------------------------------ Main_Program: #IF CalibrateFlatMode #THEN GOTO CalibrateFlat ' used to find the zero G point of your ' robot and sensor installation ' Use #DEFINES to turn it on or off. #ENDIF DO TOGGLE CycleIndicator ' indicate speed of loop on pin ' (typically 12 -> 14ms) **** ' THIS IS NOT RELATED TO THE SERVO ' REFRESH TIME! Refresh stays at 20 ms. IF Timer_Exp = 1 THEN ' check if any timers have timed out ' read timeout alarm byte to get and ' clear it SEROUT To_Co_Proc, Baud, [119] ' send the byte command SERIN Frm_Co_Proc, Baud, [serDIn] ' timeout data comes back into serDIn 'DEBUG " delayTime = expired, serDIn = ",HEX serDIn, " i = ", DEC i,CR IF (serDIn.BIT0) THEN doneMoveFlag = 1 ' servo is finished when Timer 0 expires ' If any other timers are also in use, ' check them here. ENDIF ' IF Timer_Exp = 1 GOSUB Do_Vision ' look for obstacles with IR GOSUB Do_Bumper ' check for obstacles with bumpers #IF (SwayMode = 0) #THEN ' Use #DEFINES to turn sway on or off. GOSUB New_Movemt ' gets next move from table #ENDIF IF (doneMoveFlag = 1) THEN ' done moving - lookup new move #IF (SwayMode = 1) #THEN MX = SwayIt ' used to sway back and forth to tune #ENDIF GOSUB New_Movemt1 ' generates basic moves from tables GOSUB Do_Movement ' generates next move from bMovmnt ELSE ' IF (doneMoveFlag = 1) GOSUB ChkTilt ' check if tilting too far ENDIF ' IF (doneMoveFlag = 1) LOOP END ' -----[ Subroutines ]---------------------------------------------------------- Do_Vision: FREQOUT LeftIRLED,1,38500 ' pulse left IRLED. sensors.BIT0 = LeftIRSensor ' store IR detector output in RAM FREQOUT RightIRLED,1,38500 ' repeat for the right IR pair. sensors.BIT1 = RightIRSensor ' Check if currently doing an infrared move IF ((leftIR_Flag = 1) & (rightIR_Flag = 1)) THEN See_Both IF (leftIR_Flag = 1) THEN See_Left ' is left only? IF (rightIR_Flag = 1) THEN See_Right ' is right only? ' Load IR detector output values into the lower 2 bits of the sensors ' variable, storing a number between 0 and 3 that the BRANCH command ' can execute the appropriate routine. BRANCH sensors,[See_Both,See_Right,See_Left,See_None] See_Both: newMove = PivR rightIR_Flag = 1 ' flag: 1 = something on right side leftIR_Flag = 1 ' flag: 1 = something left side SEROUT To_Co_Proc, Baud, [LeftVisionLED_On] ' turn on LED SEROUT To_Co_Proc, Baud, [RightVisionLED_On] ' turn on LED RETURN See_Right: newMove = PivL rightIR_Flag = 1 ' flag: 1 = something on right side SEROUT To_Co_Proc, Baud, [RightVisionLED_On] ' turn on LED RETURN See_Left: newMove = PivR leftIR_Flag = 1 ' flag: 1 = see something left side SEROUT To_Co_Proc, Baud, [LeftVisionLED_On] ' turn on LED RETURN See_None: newMove = Fwd RETURN '--------------- Do_Bumper: DIRS = prepBumper OUTS = bothLEDsOff ' Bumpers on each foot. A hit causes Toddler to back and turn. ' Code asssumes leg that hit is on the leg that is moving ' (while leaning on the other leg). ' A hit backs up the hit leg right away then steps back 1 step ' and does a pivot away. ' check for old bumper hits on left and right IF (leftBumpFlag = 1) THEN Bump_LeftA IF (rightBumpFlag = 1) THEN Bump_RightA ' check for new bumper hits now IF (LeftBumper = 0) THEN Bump_Left ' check for bumper hit on left IF (RightBumper = 0) THEN Bump_Right ' check for bumper hit on right DIRS = ctrlLEDs OUTS = bothGreen RETURN Bump_Left: 'DEBUG "bump left", cr ' sends only once on change SEROUT To_Co_Proc, Baud, [StrideServoPos] ' read the stride position SERIN Frm_Co_Proc, Baud, [serDIn] ' data comes back into serDIn CurrentStride = serDIn ' set the current position there ' for new delay calculations Bump_LeftA: leftBumpFlag = 1 ' flag indicates bump response move newMove = BumpL DIRS = ctrlLEDs OUTS = leftRed GOTO Clr_Vision ' goes to Clr_Vision Bump_Right: 'DEBUG "bump right", cr SEROUT To_Co_Proc, Baud, [StrideServoPos] ' read the stride position SERIN Frm_Co_Proc, Baud, [serDIn] ' data comes back into serDIn CurrentStride = serDIn ' set the current position there ' for new delay calculations Bump_RightA: rightBumpFlag = 1 newMove = BumpR DIRS = ctrlLEDs OUTS = rightRed ' falls through to Clr_Vision '--------------- Clr_Vision: ' reset vision LEDs and flags IF rightIR_Flag = 1 THEN rightIR_Flag = 0 ' flag: 1 = see something on right side SEROUT To_Co_Proc, Baud, [RightVisionLED_Off] ' turn off LED ENDIF ' IF was used to prevent constant ' serial transmissions IF leftIR_Flag = 1 THEN leftIR_Flag = 0 ' flag: 1 = see something left side SEROUT To_Co_Proc, Baud, [LeftVisionLED_Off] ' turn off LED ENDIF RETURN '--------------- New_Movemt: ' sequences for forward motion IF (newMove <> currentMove) THEN ' new DATA if movement changed 'DEBUG cr, "start NewMovemt; newMove = ", DEC newMove 'DEBUG ", currentMove = ", DEC currentMove, CR doneMoveFlag = 1 ' stop current move wherever it is currentMove = newMove ' When customizing the program, here is where to integrate between ' moves. You can make the transitions reasonable by using the existing ' Mx value and MxOffset to compute the proper MxOffset to start into ' the next move. IF ((newMove = Fwd) OR (newMove = BumpL) OR (newMove = BumpR)) THEN MxOffset = 0 ' reset MxOffset value for new move ENDIF ' (newMove = Fwd) OR ' note: kept old MxOffset value for lead into pivot left or right LOOKUP newMove,[Forward,PivotLeft,PivotRight,LBumper,RBumper],Mx ' lookup movement table index ENDIF ' newMove <> currentMove RETURN '--------------- New_Movemt1: ' lookup new move 'PULSIN Yout,1,Incline ' read tilt from Memsic 'DEBUG "Incline = ", DEC Incline, "; limits L/R are ", DEC YAxisLfLimit 'DEBUG " / ", DEC YAxisRtLimit, CR READ (Mx + MxOffset), bMovmnt ' read next basic move byte 'DEBUG "Mx = ", HEX Mx, ", MxOffset = ", HEX MxOffset MxOffset = MxOffset + 1 IF (bMovmnt = xx) THEN ' end code so do new move 'DEBUG " xx = finished table", CR, CR MxOffset = 0 ' finished move sequence so restart leftBumpFlag = 0 ' clear out left bumper hit flag rightBumpFlag = 0 ' right too DIRS = ctrlLEDs OUTS = bothGreen GOSUB Clr_Vision ' reset vision LEDs and flags #IF (SwayMode = 0) #THEN GOSUB Do_Vision ' figure out new movement, GOSUB Do_Bumper GOTO New_Movemt ' get new table and offset #ELSE DEBUG CR,CR GOTO New_Movemt1 ' used to sway back and forth ' for tuning #ENDIF ENDIF ' bMovmnt = xx RETURN '--------------- Do_Movement: 'DEBUG " bMovmnt = ", HEX bMovmnt, " " ' TL = 0, TC = 1, TR = 2, SL = 4, SC = 5, SR = 6 => uses lower nibble only IF (bMovmnt.BIT2 = 1) THEN ' Stride movements doTiltFlag = 0 IF (bMovmnt.BIT1 = 1) THEN ' Stride Right newValue = RightStride 'DEBUG "SR, ", cr ELSEIF (bMovmnt.BIT0 = 1) THEN ' bMovmnt.BIT1 = 1 newValue = CenterStride 'DEBUG "SC, ", cr ELSE ' bMovmnt.BIT1 = 1, bMovmnt.BIT0 = 1 newValue = LeftStride 'DEBUG "SL, ", cr ENDIF ' bMovmnt.BIT1 = 1 IF (currentStride < newValue) THEN delayTime = newValue - currentStride ELSE delayTime = currentStride - newValue ENDIF SEROUT To_Co_Proc, Baud, [StrideServo,newValue] 'Set Servo 1 = Stride delayTime = (delayTime/StrideStep)*4 CurrentStride = newValue ELSE ' bMovmnt.BIT2 = 1 => Tilt Movements doTiltFlag = 1 IF (bMovmnt.BIT1 = 1) THEN ' Tilt Right newValue = RightTilt SEROUT To_Co_Proc, Baud, [TiltRamp,TiltStep1] 'write rate to servo 0 tiltingLeftFlag = 0 'DEBUG "TR, ", cr ELSEIF (bMovmnt.BIT0 = 1) THEN ' IF (bMovmnt.BIT1 = 1) newValue = CenterTilt SEROUT To_Co_Proc, Baud, [TiltRamp,TiltStep2] 'write rate to servo 0 doTiltFlag = 0 ' don't need to do tilt measurement 'DEBUG "TC, ", cr ELSE ' IF (bMovmnt.BIT1 = 1),bMovmnt.BIT0 = 1 newValue = LeftTilt SEROUT To_Co_Proc, Baud, [TiltRamp,TiltStep1] 'write rate to servo 0 tiltingLeftFlag = 1 'DEBUG "TL, ", cr ENDIF ' bMovmnt.BIT1 = 1 IF (currentTilt < newValue) THEN delayTime = newValue - currentTilt ELSE delayTime = currentTilt - newValue ' coming from left tilt = decrement ENDIF SEROUT To_Co_Proc, Baud, [TiltServo,newValue] 'Set Servo 0 = Tilt delayTime = (delayTime/TiltStep1)*4 ' add a ramp rate bit?????? CurrentTilt = newValue ENDIF ' bMovmnt.BIT2 = 1 IF delayTime < 255 THEN delayTime = delayTime + 1 ' to make up for fractions from divide 'DEBUG " delayTime = ", DEC delayTime, CR,LF SEROUT To_Co_Proc, Baud, [(128),delayTime] 'write Timer 0 with delay value doneMoveFlag = 0 ' reset flags ovrCntrFlag = 0 RETURN '--------------- ChkTilt: IF (doTiltFlag = 1) THEN ' The commands below can be used for charting the details ' of the Memsic sensor while in sway mode. ' Remember that they drastically slow down the sensing loop! ' Do any final tuning with the DEBUGs and these commands disabled. 'PULSIN Yout,1,Incline ' read tilt from Memsic 'SEROUT To_Co_Proc, Baud, [TiltServoPos] ' read the tilt position 'SERIN Frm_Co_Proc, Baud, [serDIn] 'DEBUG "Servo tiltpos = ", DEC3 serDIn 'DEBUG "; Incline = ", DEC Incline, "; Tilt Cntr = ", DEC tiltCounter,CR ' only do this when tilting ' The overcenter flag is used to ignore the shock of the foot ' touching the floor while going from left to right or right to left. ' That shock can cause the Memsic and tilt counter act like that it ' is finished tilting. 'ovrCntrFlag = 1 ' to test the effect of using flag IF (ovrCntrFlag = 0) THEN ' not beyond straight upright yet SEROUT To_Co_Proc, Baud, [TiltServoPos] ' read the tilt position SERIN Frm_Co_Proc, Baud, [serDIn] 'DEBUG "tiltpos = ", DEC serDIn, CR IF (tiltingLeftFlag = 1) THEN IF serDIn > ServOvrLeft THEN ovrCntrFlag = 1 'DEBUG "over center left", CR ENDIF ' serDIn < ServOvrLeft ELSE ' tiltingLeftFlag = 1 IF serDIn < ServOvrRight THEN ovrCntrFlag = 1 'DEBUG "over center right", CR ENDIF ' serDIn > ServOvrRight ENDIF ' tiltingLeftFlag = 1 tiltCounter = 0 ' reset tilt sensor counter ELSE ' ovrCntrFlag = 0 PULSIN Yout,1,Incline ' read tilt from Memsic 'DEBUG "Incline = ", DEC Incline, CR IF (tiltingLeftFlag = 1) THEN IF Incline < YAxisLfLimit THEN tiltCounter = tiltCounter + 1 'DEBUG "Tilt Counter = ", DEC tiltCounter, CR ENDIF ' Incline < ELSE ' (tiltingLeftFlag = 1) IF Incline > YAxisRtLimit THEN tiltCounter = tiltCounter + 1 'DEBUG "Tilt Counter = ", DEC tiltCounter, CR ENDIF ' Incline > ENDIF ' (tiltingLeftFlag = 1) IF (tiltCounter > escVal) THEN ' at limit of tilt, stop tilting 'DEBUG "Max Tilt",CR SEROUT To_Co_Proc, Baud, [TiltServoPos] ' read the tilt position SERIN Frm_Co_Proc, Baud, [CurrentTilt] ' make it current tilt value SEROUT To_Co_Proc, Baud, [TiltServo,CurrentTilt] ' set Servo 0 to stop at current Tilt SEROUT To_Co_Proc, Baud, [(128),0] ' clear Timer 0 with no alarm set doneMoveFlag = 1 tiltCounter = 0 ENDIF ' tiltCounter > escVal ENDIF ' ovrCntrFlag = 0 ENDIF ' doTiltFlag = 1 RETURN ' -----[ Calibration Routines - can be removed after robot is tuned ]----------- ' The serial cable is too stiff for sway to work with it installed. ' If you want to see the values while Toddler is moving, you can make an ' extension with flexible wire and 2 DB9 connectors. That will let Toddler ' move more freely while still logging the values. Wire pins 2,3,4, & 5 from ' the male to the female connector. Wire pin 6 to pin 7 on the female ' connector only. Sway: #IF (SwayMode = 1) #THEN 'PULSIN Yout,1,Incline ' read tilt from Memsic 'DEBUG "Incline = ", DEC Incline, "; limits are ", DEC YAxisRtLimit 'DEBUG DEC YAxisLfLimit, CR IF (bMovmnt = TR) THEN ' toggle between tilt left and right bMovmnt = TL 'DEBUG ", Done Tilt Right, limit was ", DEC YAxisRtLimit, CR ELSE bMovmnt = TR 'DEBUG ", Done Tilt Left, limit was ", DEC YAxisLfLimit, CR ENDIF ' bMovmnt = TR #ENDIF RETURN '--------------- ' Put the Toddler on a flat level surface to measure the zero point. ' If the value is too far off, the sensor may not be seated correctly ' on the PCB. You can also tilt the Toddler and measure the tilt values ' to get a feel for how they vary versus Toddler tilt angles. Lightly ' tap on Toddler while flat and level to see how shocks affect the readings. ' To use CalibrateFlat, you have to uncomment the DEBUG statement. It is ' commented out so a DEBUG window doesn't come up every time the program ' is "Run". CalibrateFlat: PULSIN Yout,1,Incline ' read tilt from Memsic 'DEBUG "Incline = ", DEC Incline, CR ' my Toddler reads 2508 on flat PAUSE 100 GOTO CalibrateFlat ' loop continuously