robot.py

00001 # Anything that starts with a # is a comment!
00002 # Use lots of comments in your code to explain what you're doing.
00003 
00004 # Use the PIC-specific Python libraries
00005 import pic24_dspic33 as pic
00006 
00007 # Monitor RAM usage
00008 import sys
00009 print "\n\nRAM usage: (free, total): ", sys.heap()
00010 
00011 
00012 
00013 ###############################################################
00014 # Set up line sensors, distance sensor, motors 
00015 ###############################################################
00016 
00017 # Line sensors are digital inputs. Create one.
00018 print "Testing line sensors.",
00019 # For port and pin, a label of RA1 is port = 0 (A), pin = 1;
00020 #                   a label of RB9 is port = 1 (B), pin = 9.
00021 #                             port pin isInput
00022 line_sensor_left =   pic.digital_io(1,   7,  True)
00023 line_sensor_middle = pic.digital_io(1,   8,  True)
00024 line_sensor_right =  pic.digital_io(1,   9,  True)
00025 print "Line sensor 4: Is a black line present?",
00026 print line_sensor_middle.get()
00027 
00028 # The distance sensor is an analog input. Create one.
00029 print "Distance sensor: voltage is",
00030 # The only argument to analog_input is the ANx number of
00031 # the analog pin to use. Below, the distance sensor is
00032 # connected to AN4.
00033 dist_sensor = pic.analog_input(4)
00034 print dist_sensor.getVoltage(), "V"
00035 
00036 # The servos are controlled by pulse-width modulation.
00037 print "Driving left..."
00038 #                    Frequency  isTimer2  OC module  RPx pin number
00039 left_wheel  = pic.pwm(50,        True,     1,         6)
00040 right_wheel = pic.pwm(50,        True,     2,         5)
00041 # The motor speed is controlled by the parameter to the setCounts method.
00042 # In this case, 1 count = 1.6 us.
00043 # Values to use:
00044 #   Max forward: 1300 us = 1300/1.6 counts = 813 counts
00045 #   Stopped    : 1500 us = 1500/1.6 counts = 938 counts
00046 #   Max reverse: 1700 us = 1700/1.6 counts = 1063 counts
00047 # Values between these maxima control the forward/reverse speed.
00048 #
00049 # NOTES:
00050 # - The actual counts to make each wheel stop vary slightly
00051 #   from wheel to wheel. So, tune these until the wheels actually do stop.
00052 # - Because the wheels are physically installed in opposite directions,
00053 #   "forward" for one wheel makes it spin in reverse.
00054 # To keep track of this, use variables to record forward/stop/reverse
00055 # for each wheel.
00056 left_wheel_forward  = 1063
00057 left_wheel_stopped  = 942
00058 left_wheel_backward = 813
00059 
00060 right_wheel_forward  = 813
00061 right_wheel_stopped  = 938
00062 right_wheel_backward = 1063
00063 
00064 #def drive_forward():
00065 #    left_wheel.setCounts(left_wheel_forward)
00066 #    right_wheel.setCounts(right_wheel_forward)
00067     
00068 #def stop_driving():
00069 #    left_wheel.setCounts(left_wheel_stopped)
00070 #    right_wheel.setCounts(right_wheel_stopped)
00071 
00072 #left_wheel.setCounts(813)
00073 #right_wheel.setCounts(1063)
00074 
00075 
00076 
00077 ###############################################################
00078 # Main loop
00079 ###############################################################
00080 
00081 while True:
00082     # Stop if object is close
00083     if dist_sensor.getVoltage() > 2.5:
00084         left_wheel.setCounts(left_wheel_stopped)
00085         right_wheel.setCounts(right_wheel_stopped)
00086     # Stop if line is detected
00087     elif line_sensor_middle.get():
00088         left_wheel.setCounts(left_wheel_stopped)
00089         right_wheel.setCounts(right_wheel_stopped)
00090     # Keep running
00091     else:
00092         left_wheel.setCounts(left_wheel_forward)
00093         right_wheel.setCounts(right_wheel_forward)
00094 
00095     # Print out line following diags
00096     if line_sensor_left.get():
00097         print "X",
00098     else:
00099         print ".",
00100     if line_sensor_middle.get():
00101         print "X",
00102     else:
00103         print ".",
00104     if line_sensor_right.get():
00105         print "X"
00106     else:
00107         print "."
00108 
00109     # Delay some
00110     time = sys.time()
00111     while time + 10 > sys.time():
00112         pass
00113     
00114 
00115 
00116 
00117 
00118 
00119 
00120 

Generated on Mon Oct 18 07:40:47 2010 for Python-on-a-chip by  doxygen 1.5.9