'************************************************************ '** '** pwm.bsc : sample wall-following using two PWM motors '** '** Copyright (c) 2004, Kyle A. York '** All rights reserved '** '************************************************************ PIC 16F877 FREQ 20. OSCILLATOR RC ' ' we've two motors (left, right) ' constant byte MOTOR_LEFT = 0, MOTOR_RIGHT = 1 sbyte speed, speed_left, speed_right byte duty, duty_last(2), motor ' ' we've two eyes (front, right) ' constant byte EYE_FRONT = 3, EYE_RIGHT = 2 byte dist_front, dist_right byte look_dir, look_dst ' ' a few distance thresholds; my measurements show roughly the following: ' inches upper 8 bits ' 1* 58 ' 2* 100 ' 3* 140 ' 4* 120 ' 5 100 ' 6 85 ' 7 72 ' 8 66 ' 9 62 ' 10 55 ' 11 50 ' 12 48 ' ~ 15 ' ' * dist <= 4'' are unreliable as the detection range is ' 10...80cm (~4..31in) ' note : the sensors seem to wander around +/- 5 of the above values ' constant byte THRESHOLD_FRONT = 85 ' if dist > this, we're blocked constant byte THRESHOLD_RIGHT_TOO_CLOSE = 90 ' if dist > this, we're too close constant byte THRESHOLD_RIGHT_CLOSE = 85 ' if dist > this we need to jog constant byte THRESHOLD_RIGHT_TOO_FAR = 80 ' if dist < this, we're too far constant byte THRESHOLD_RIGHT_FAR = 85 ' if dist < this, we need to jog byte action ' the current motor action (used for debugging) constant byte ACTION_STOP = 0 constant byte ACTION_FORWARD = 1 constant byte ACTION_REVERSE = 2 constant byte ACTION_SWEEP_LEFT = 3 constant byte ACTION_SWEEP_RIGHT = 4 constant byte ACTION_SWEEP_LEFT_QUICK = 5 constant byte ACTION_SWEEP_RIGHT_QUICK = 6 constant byte ACTION_ROTATE_LEFT = 7 constant byte ACTION_ROTATE_RIGHT = 8 clear ' ****** Init all PORTS ****** SetPort A , %111111 ' a is all input SetPort B , %000000 ' b is all output SetPort C , %000000 ' c is all output SetPort D , %000000 ' d is all output SetPort E , %111 ' e is all input ' ' setup this for any debug print statements ' USART_Config TX, 8, 38.4, 0 ' ' setup the motors to full stop ' PinLow b , 4 PinLow b , 5 PWM_Config 50.0, 0, 3, 0 duty_last(MOTOR_LEFT) = 0 duty_last(MOTOR_RIGHT) = 0 ' ' ' go forward until we see something. we should always start out ' perpendicular to a wall, but that's not really necessary. since ' we can only see front & right, we should always be aimed such ' that either the front or right sensor will eventually see something ' ' ' simple test : forward 2s, pivot left 2s, pivot right 2s, backward 2s ' stop 2s ' ' start with the motors, full speed in each direction one at a time ' ' nb: any code surrounded by ``if 0'' is removed by the dead code ' detector ' if 1 then print "left, full reverse\r\n" call motor_speed_set(MOTOR_LEFT, -100) call delay_5s() print "left, full forward\r\n" call motor_speed_set(MOTOR_LEFT, 100) call delay_5s() call motor_speed_set(MOTOR_LEFT, 0) print "right, full reverse\r\n" call motor_speed_set(MOTOR_RIGHT, -100) call delay_5s() print "right, full forward\r\n" call motor_speed_set(MOTOR_RIGHT, 100) call delay_5s() call motor_speed_set(MOTOR_RIGHT, 0) end if if 0 then do CALL forward() call delay_5s() call rotate_left() call delay_5s() call rotate_right() call delay_5s() call reverse() call delay_5s() call stop() call delay_5s() while(1) end if if 0 then do call look(EYE_FRONT, dist_front) call look(EYE_RIGHT, dist_right) print "front=", dist_front, " right=", dist_right, "\r\n" delay 333.0, 0 ' 1/3 sec delay while (1) end if ' ' start by simply going forward until something appears either ' in front to the side. ' call forward() do call look(EYE_FRONT, dist_front) call look(EYE_RIGHT, dist_right) while (dist_front < THRESHOLD_FRONT) & (dist_right < THRESHOLD_RIGHT_CLOSE) ' ' we've something to guide us, let's go for it! ' general plan: ' 1. blocked in front --> pivot ' 2. much too close to the wall ' sweep left ' 3. a bit too close to the wall ' sweep left quick ' 4. a bit too far from the wall ' sweep right quick ' 5. much too far from the wall ' sweep right ' do call look(EYE_FRONT, dist_front) call look(EYE_RIGHT, dist_right) if (dist_front > THRESHOLD_FRONT) then call front_blocked() ' we're blocked, pivot until not blocked elif (dist_right > THRESHOLD_RIGHT_TOO_CLOSE) then call sweep_left() ' far too close to the wall, sweep elif (dist_right > THRESHOLD_RIGHT_CLOSE) then call sweep_left_quick() ' a bit too close to the wall, jog elif (dist_right < THRESHOLD_RIGHT_TOO_FAR) then call sweep_right() ' far too far from the wall, sweep elif (dist_right < THRESHOLD_RIGHT_FAR) then call sweep_right_quick() ' a bit too far from the wall, jog end if while (1) ' an infinite loop ' ' read the sensors ' look_dir = EYE_FRONT or EYE_RIGHT ' look_dst = destination for result ' channel 2 = front ' channel 3 = right ' proc look(look_dir, look_dst) ' channel, power, left-justify, control, no interrupt ADC_Config look_dir, 1, 0, 0, 0 Delay 0.01, 0 ADC_Start Delay 0.04, 0 ADC_Wait 'ADC_Store look_dst 'look_dst = look_dst / 256 ' only use the MSB look_dst = _adresh 'i'm only interested in the MSB ADC_Off end proc ' ' set the speed for motor ' -100 <= speed <= 100 ' motor = motor_left or motor_right ' ' note: pwm1/b<4> = left motor speed/direction ' pwm2/b<5> = right motor speed/direction ' proc motor_speed_set(motor, speed) if (speed < 0) then pinhigh b, 4 + motor duty = speed + 100 else pinlow b, 4 + motor duty = speed end if print "motor_speed_set speed=", speed, " motor=", motor print " duty=", duty, " duty_last=", duty_last(motor), "\r\n" if (duty <> duty_last(motor)) then duty_last(motor) = duty pwm_duty duty, 1 + motor end if end proc ' ' set the speed for both motors ' -100 <= (speed_left, speed_right) <= 100 ' proc speed_set(speed_left, speed_right) call motor_speed_set(MOTOR_LEFT, speed_left) call motor_speed_set(MOTOR_RIGHT, speed_right) end proc ' ' sweep to the left (wide arc) ' PROC sweep_left() if (ACTION_SWEEP_LEFT <> action) then action = ACTION_SWEEP_LEFT print "left\r\n" call speed_set(50, 100) end if END PROC ' ' sweep to the right (wide arc) ' PROC sweep_right() if (ACTION_SWEEP_RIGHT <> action) then action = ACTION_SWEEP_RIGHT print "right\r\n" call speed_set(100, 50) end if END PROC ' ' move slightly to the left ' PROC sweep_left_quick() if (ACTION_SWEEP_LEFT_QUICK <> action) then action = ACTION_SWEEP_LEFT_QUICK print "sweep left quick\r\n" call speed_set(95, 100) end if END PROC ' ' move slightly to the right ' PROC sweep_right_quick() if (ACTION_SWEEP_RIGHT_QUICK <> action) then action = ACTION_SWEEP_RIGHT_QUICK print "sweep right quick\r\n" call speed_set(100, 95) end if END PROC ' ' all stop ' PROC stop() if (ACTION_STOP <> action) then action = ACTION_STOP print "stop\r\n" call speed_set(0,0) end if END PROC ' ' full forward ' PROC forward() if (ACTION_FORWARD <> action) then action = ACTION_FORWARD print "forward\r\n" call speed_set(100, 100) end if END PROC ' ' full reverse ' PROC reverse() if (ACTION_REVERSE <> action) then action = ACTION_REVERSE print "reverse\r\n" call speed_set(-100, -100) endif END PROC ' ' rotate (pivot) left ' PROC rotate_left() if (ACTION_ROTATE_LEFT <> action) then action = ACTION_ROTATE_LEFT print "rotate left\r\n" call speed_set(-100, 100) end if END PROC ' ' rotate (pivot) right ' PROC rotate_right() if (ACTION_ROTATE_RIGHT <> action) then action = ACTION_ROTATE_RIGHT print "rotate right\r\n" call speed_set(100, -100) end if END PROC ' ' front is blocked, rotate until the right sensor is in position ' first, the front sensor will get farther & farther away ' proc front_blocked() call rotate_left() do call look(EYE_FRONT, dist_front) while (dist_front > 40) ' ' now the right eye should be in range, continue rotating until ' it no longer it. ' do call look(EYE_RIGHT, dist_right) while (dist_right < THRESHOLD_FRONT) end proc proc delay_5s() delay 5000.0, 0 end proc