'======================================================================== ' MAE 171B Program for COSMOS sensor box to catch balls measuring velocity ' June 2005 ' Program by Sean Wu, Jan Schellenberger, Alex Simpkins ' Project Team members Kyaw Win, Lowell Tejada, Ernesto Garcia '======================================================================== 'enable an 8 bit timer interrupt, scaled by 8, 'so every (256*8)/5e6 sec there is an interrupt 'associate interrupt with the subroutine timer_int_func oninterrupt tmr0int, timer_int_func 'setup the timer scaling settmr0 tmr0int8 'enable the interrupt enable tmr0int '==========================constants===================================== DISTANCE1 con 1270000 'distance of sensor gate in meters (times 1e8) 12.7mm measured 'lcd display constants lcd con 15 '==========================variables===================================== 'options are : 'bit 0 to 1 'byte 0 to 255 'sbyte -127 to 128 'word 0 to 65535 'sword -32767 to 32768 'long -2147483647 to 2147483648 'float (*not used in this course*) index var byte position var sword position2 var sword positionlast var sword temp var word v var word ball_block_flag1 var bit ball_block_flag2 var bit time1 var long time1temp var long display_sensor1 var bit tog_switch1 var bit decimalselect1 var bit velocity var long temp1 var word max_dat var word '=========================initialization================================== 'set ball occlusion flags to 0 ball_block_flag1 = 0 ball_block_flag2 = 0 'reset the position of the servo to a middle position position = 0 temp1 = 0 velocity = 0 positionlast = 0 'clear the lcd display serout lcd,N2400,[12] 'turn on the backlight for the lcd 'serout lcd,N2400,[14] '========================================= '===============================main program code=============================== 'an infinite loop to read the one ball sensor and react if necessary, otherwise 'skip that code and loop Readsensors: 'SENSOR 1 - using the first sensor connection 'check for gate 1 if in0 = 0 and ball_block_flag1 = 0 then 'ball is blocking sensor time1temp = 0 'ball just passed gate, don't reset time... ball_block_flag1 = 1 endif 'check for gate 2 if in1 = 0 and in0 = 1 and ball_block_flag1 = 1 then 'measure time by taking the current delta T, we assign it to a different variable 'to avoid adding extra time steps when not necessary. We could also stop the 'timer. time1 = time1temp ball_block_flag1 = 0 velocity = DISTANCE1/time1 'Compute the position to move the sensor to, scaled to be the max and min range ' position = 690 - velocity*10/22 position = -velocity/10*2 + 388 'enforce boundaries for the servo, because it has limited range if position>450 then position =450 elseif position <-450 position =-450 endif 'move the servo to the position calculated... servo p9, position, 20 'then reset it... servo p9, 0, 15 'write it to the LCD screen... 'clear the screen 'write the position to the first line... ' serout lcd, N2400,["Pos is ",sdec position] 'move to the beginning of the second line... ' serout lcd, N2400,[13] serout lcd, N2400, [12] serout lcd, N2400, [dec velocity, "m/s x 1000"] serout lcd, N2400, [13] serout lcd, N2400, [sdec position," servo2 cnts"] 'write the velocity to the second line... ' serout lcd, N2400,["Vel is ",dec velocity] endif 'if sensors don't detect anything, and the knob moved, move the servo if in0=1 and in1=1 and ball_block_flag1 = 0 then adin AX0, 2, AD_RON, v position2=-(900-(v-300)) 'position = 1023-v*2 if abs(positionlast-v)>10 then serout lcd, N2400, [12] temp = v*50/1023 temp = 50 - temp serout lcd, N2400, [12] serout lcd, N2400, [dec temp/10,".", dec (temp-temp/10*10), " Volts" ] 'serout lcd, N2400, [dec 5-v*5/1023,".", dec (((1023-v)*50/1023)-((1023-v)*50/1023/10*10)), " Volts" ] if position2 < -550 then position2 = -550 elseif position2 > 0 position2 = 0 endif servo p10, position2, 5 positionlast = v serout lcd, N2400, [13] serout lcd, N2400, ["Servo1 cnts", sdec position2] endif endif 'go back to read the sensors... goto Readsensors '===============TIME INCREMENT================== 'this is an interrupt function, occurring every 0.41 msec, regardless of what the pic is doing timer_int_func: 'we'll multiply by 100 and round if in1 = 1 then time1temp = time1temp + 41 endif resume 'resumes the normal program execution