Advertisement
KRITSADA

2 GP2D120 Compare Robot Running

Mar 15th, 2018
824
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. ' {$STAMP BS2sx}
  2. ' {$PBASIC 2.5}
  3. ' {$PORT COM1}
  4.  
  5. CH       VAR        Nib
  6. CNT     VAR     Byte
  7. ADC      VAR        Word
  8. ADC1     VAR        Word
  9. ADC2     VAR       Word
  10. Init:    DIRD = %1111
  11.  
  12.          PAUSE 1000
  13. Main:    GOSUB GP2
  14.          IF ((ADC1<10) AND (ADC2<10)) THEN
  15.               GOSUB S_Left
  16.          ENDIF
  17.         IF (ADC1 > (ADC2+100)) THEN
  18.               GOSUB S_Left
  19.          ENDIF
  20.         IF (ADC2 > (ADC1+100)) THEN
  21.               GOSUB S_Right
  22.          ENDIF
  23.          IF ((ABS(ADC1-ADC2)) < 100) THEN
  24.               GOSUB Forward
  25.          ENDIF
  26.          CH=1 : GOSUB RD_ADC
  27.          IF (ADC>350) THEN
  28.                 GOSUB Backward
  29.               PAUSE 1000
  30.               GOSUB S_right
  31.               PAUSE 2000
  32.          ENDIF
  33. GOTO Main
  34.  
  35.  
  36. GP2:     CH=0 : GOSUB RD_ADC : ADC1 = ADC
  37.         CH=2 : GOSUB RD_ADC : ADC2 = ADC
  38.          RETURN
  39.  
  40. '+++++ Analog to Digital Converter Procedure ++++++++++
  41. RD_ADC: LOW 10: PAUSE 1: HIGH 10                    ' Send Acknowledge
  42.             SEROUT 10,240,[CH]                          ' Send Select Chip
  43.             SERIN 10,240,[ADC.BYTE0,ADC.BYTE1]          ' Read ADC
  44.          RETURN
  45. '++++++++++++++++++++++++++++++++++++++++++++++++++++++
  46.  
  47. '+++++ Generate Sound +++++++++++++++++++++++++++++++++
  48. Beep:       FREQOUT 11,300,800                  ' Sound subroutine
  49.             RETURN
  50. '++++++++++++++++++++++++++++++++++++++++++++++++++++++
  51.  
  52. '+++++ Movement Procedure +++++++++++++++++++++++++++++
  53. Forward:    OUTD = %1010 : RETURN   ' Motor A --> and Motor B --> Go Forward
  54. Backward:   OUTD = %0101 : RETURN       ' Motor A <-- and Motor B <-- Go Backward
  55. S_Left:     OUTD = %0110 : RETURN   ' Robot Spin Left
  56. S_Right:    OUTD = %1001 : RETURN   ' Robot Spin Right
  57. Motor_OFF:  OUTD = %0000 : RETURN   ' Stop All Motor
  58. '++++++++++++++++++++++++++++++++++++++++++++++++++++++
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement