' ========================================================================= ' ' File...... explorer.sxb ' Purpose... explorer robot ' Author.... David Mcanulty ' E-mail.... dave000@hellspark.com ' ' ========================================================================= ' ------------------------------------------------------------------------- ' Program Description ' ------------------------------------------------------------------------- ' ------------------------------------------------------------------------- ' Device Settings ' ------------------------------------------------------------------------- DEVICE SX28,TURBO,BANKS8,OSCHS3,OPTIONX,BOR42 FREQ 50_000_000 ' ------------------------------------------------------------------------- ' IO Pins ' ------------------------------------------------------------------------- ' outputs to motor controller chips Left_Motor_F VAR RA.0 Left_Motor_R VAR RA.1 Right_Motor_F VAR RA.2 Right_Motor_R VAR RA.3 ' Ground outputs to IR emmitors Front_IR_Emit_L VAR RB.0 Front_IR_Emit_R VAR RB.1 'auto_enable is generated by the "turbo" function of the transmitte/reciever board Auto_Enable VAR RB.2 'This is the 38khz modulated freq IR_Emit VAR RB.3 'This is the front mounted IR reciever module Front_IR_Detect VAR RB.4 ' Status leds for debuging only Left_Motor_F_Status VAR RC.5 Left_Motor_R_Status VAR RC.4 Right_Motor_F_Status VAR RC.3 Right_Motor_R_Status VAR RC.2 IR_Emit_Status VAR RC.1 Front_IR_Detect_Left_Status VAR RB.7 Front_IR_Detect_Center_Status VAR RB.6 Front_IR_Detect_Right_Status VAR RB.5 ' ------------------------------------------------------------------------- ' Constants ' ------------------------------------------------------------------------- Yes CON 0 ' ir sensor logic is flipped 0v is detect this helps keep my sanity No CON 1 Motor_Delay CON 250 '#ms long to leave the motors on Loop_Delay CON 100 '#ms to delay logic loop 'Motor_Delay CON 1 'for sxsim #ms long to leave the motors on 'Loop_Delay CON 1 'for sxsim #ms to delay logic loop Motor_Pulses CON 1 ' how many times to pulse the motor ' ' ------------------------------------------------------------------------- ' Variables ' ------------------------------------------------------------------------- ir_in_left VAR Byte ir_in_right VAR Byte ir_in_center VAR Byte tmpW1 VAR Word i VAR Byte ' ------------------------------------------------------------------------- ' INTERRUPT ' ------------------------------------------------------------------------- INTERRUPT NOPRESERVE 76_000 IR_Emit = ~IR_Emit ' Toggle pin state RETURNINT ' ========================================================================= PROGRAM Start ' ========================================================================= Pgm_ID: DATA "Explorer", 0 ' ------------------------------------------------------------------------- ' Subroutine Declarations ' ------------------------------------------------------------------------- DELAY_MS SUB 1, 2 ' delay in milliseconds ' ------------------------------------------------------------------------- ' Program Code ' ------------------------------------------------------------------------- Start: ' initialization code here TRIS_A = %0000 ' make input or output (0=output 1=input) PLP_A = %1111 ' 0=pulls-up unused pins (unused should be inputs) TRIS_B = %00010100 ' 2,4 = input, the rest are outputs ST_B = %11101111 ' set input 4 to schmitt trigger PLP_B = %11111111 ' no pullups TRIS_C = %00000000 ' all outputs PLP_C = %11111111 ' no pullups 'Default motors to coast mode Left_Motor_F = 0 Left_Motor_R = 0 Right_Motor_F = 0 Right_Motor_R = 0 IR_emit_Status = 0 Front_IR_Emit_L =1 Front_IR_Emit_R =1 Main: DO DELAY_MS Loop_Delay ' clear all status leds Left_Motor_F_Status = 0 Left_Motor_R_Status = 0 Right_Motor_F_Status = 0 Right_Motor_R_Status = 0 Front_IR_Detect_Center_Status = 0 Front_IR_Detect_Left_Status = 0 Front_IR_Detect_Right_Status = 0 ' read IR detector status into a variable (so it dosn't change mid loop) IR_emit_Status = 1 ' Debug var Front_IR_Emit_L=0 'output 38khz wave to left emittor DELAY_MS 1 'give it time to emit 38x10 pulses IR_IN_LEFT = Front_IR_Detect 'read result into variable Front_IR_Emit_L=1 'force the led off (other pin is +) Front_IR_Emit_R=0 'output 38khz wave to right emittor DELAY_MS 1 IR_IN_RIGHT = Front_IR_Detect Front_IR_Emit_R=1 'force the led off (other pin is +) IR_IN_CENTER = IR_IN_LEFT + IR_IN_RIGHT 'sensor is 0v for detect so 0+0=0= yes IR_emit_Status = 0 IF IR_IN_CENTER = yes THEN ' yes is 0, 0+0 = 0 = yes ...nice eh? IR_IN_LEFT = no 'its a center, clear for below IR_IN_Right= no 'i could do an if else, but blah ' oh no, both detectors see something, run away! Front_IR_Detect_Center_Status = 1 IF Auto_Enable = 1 THEN FOR i = 1 TO Motor_Pulses Left_Motor_R_Status = 1 Right_Motor_R_Status = 1 Left_Motor_R = 1 Right_Motor_R = 1 DELAY_MS Motor_Delay Left_Motor_R = 0 Right_Motor_R = 0 'DELAY_MS Motor_Delay NEXT ENDIF ELSEIF ir_in_left = yes THEN Front_IR_Detect_Left_Status = 1 IF Auto_Enable = 1 THEN FOR i = 1 TO Motor_Pulses Left_Motor_F_Status = 1 Left_Motor_R_Status = 0 Left_Motor_F = 1 Left_Motor_R = 0 DELAY_MS Motor_Delay Left_Motor_F = 0 Left_Motor_R = 0 'DELAY_MS Motor_Delay NEXT ENDIF ELSEIF ir_in_right = yes THEN Front_IR_Detect_right_Status = 1 IF Auto_Enable = 1 THEN FOR i = 1 TO Motor_Pulses right_Motor_F_Status = 1 right_Motor_R_Status = 0 right_Motor_F = 1 right_Motor_R = 0 DELAY_MS Motor_Delay right_Motor_F = 0 right_Motor_R = 0 'DELAY_MS Motor_Delay NEXT ENDIF ELSE ' didn't see anything, charge confidently foward whats the worst that could happen? IF Auto_Enable = 1 THEN FOR i = 1 TO Motor_Pulses Left_Motor_F_Status = 1 Right_Motor_F_Status = 1 Left_Motor_F = 1 Right_Motor_F = 1 DELAY_MS Motor_Delay Left_Motor_F = 0 Right_Motor_F = 0 'DELAY_MS Motor_Delay NEXT ENDIF ENDIF Loop GOTO Main ' ------------------------------------------------------------------------- ' Subroutine Code ' ------------------------------------------------------------------------- ' Use: DELAY_MS msecs DELAY_MS: IF __PARAMCNT = 1 THEN tmpW1 = __PARAM1 ' save byte value ELSE tmpW1 = __WPARAM12 ' save word value ENDIF PAUSE tmpW1 RETURN