test.gcb

#CHIP 16F887

// Components
#DEFINE MotorL PortB.2
#DEFINE MotorL2 PortB.3
#DEFINE MotorR PortB.4
#DEFINE MotorR2 PortB.5
#DEFINE WallDet1 PortA.0 // D2, AN0
#DEFINE WallDet2 PortA.1 // D3, AN1
#DEFINE LineDet PortA.3 // D5, AN3
#DEFINE FireDet PortA.5 // D7, AN4
#DEFINE FanMotor PortB.1

// LCD display
#DEFINE LCD_IO 4
#DEFINE LCD_RS portd.0
#DEFINE LCD_RW portd.1
#DEFINE LCD_Enable portd.2
#DEFINE LCD_DB4 portd.4
#DEFINE LCD_DB5 portd.5
#DEFINE LCD_DB6 portd.6
#DEFINE LCD_DB7 portd.7

// Pinout
Dir MotorL Out
Dir MotorL2 Out
Dir MotorR Out
Dir MotorR2 Out
Dir LineDet In
Dir WallDet1 In
Dir WallDet2 In
Dir FireDet In
Dir FanMotor Out

// Variables
Dim wallDist as byte
Dim deltaWallDist as byte
Dim fireValue as byte
Dim lineCount as byte



// Void setup
FanMotor = true
LineDet = true
lineCount = 0
// Motors on by default
MotorL = false
MotorR = false
MotorL2 = false
MotorR2 = false


Do
    
Loop

Sub LeftTurn90
  MotorL = false
  MotorR = false
  wait 300 ms
  MotorL = false
  MotorL2 = true
  MotorR = true
  MotorR2 = false
  wait 405 ms
//   370 ms
  MotorL = false
  MotorR = false
  MotorL2 = false
  MotorR2 = false
  wait 300 ms
end sub

Sub RightTurn90
  MotorL = false
  MotorR = false
  wait 300 ms
  MotorL = true
  MotorL2 = false
  MotorR = false
  MotorR2 = true
  wait 420 ms
  MotorL = false
  MotorR = false
  MotorL2 = false
  MotorR2 = false
  wait 300 ms
end sub