Latest Update: SpamButcher 2.1xd  
|
 Cutting Edge Spam Elimination Melty B Homepage...
Melty B has been ported to C as part of the Open Melt project. The source (Bascom AVR) for the current test platform is listed below for your use.
'Melty B - Translational Drift / Melty Brain combat robot
'www.spambutcher.com
'This code is provided for your use without warranty / etc...
Hardware:
'Atmega 168 / 20MHZ crystal
'Bascom AVR Compiler (need commercial version due to size)
'Motor control: STMicroelectronics BU941ZT Darlington drivers (Mouser.com)
'Accelerometer: Freescale 200G MMA2301EG (Mouser.com)
'MCU Board: Pololu Baby Orangutan / Mega168 (pololu.com)
'portb.0 - Throttle (87 = low, 115 = middle, 148 = high)
'portb.3 - Left / Right (425 = left, 469 = center, 541 = right)
'portb.4 - Forward / Back (145 = forward, 114 = center, 83 = back)
'adc.5 accelerometer
'portd.6 - heading indicator LED
'portd.3 - motor 1
'portd.4 - motor 2
$crystal = 20000000 ' used crystal frequency
$hwstack = 32 ' default use 32 for the hardware stack
$swstack = 10 ' default use 10 for the SW stack
$framesize = 40 ' default use 40 for the frame space
$baud = 9600
Config Portd = Output
Config Portb = Input
Config Adc = Single , Prescaler = Auto , Reference = Internal 'accelerometer is compared to internal 2.5v voltage
Dim A As Byte 'general variables
Dim X As Long
Dim Accel_raw_data As Word 'raw accelerometer data
Dim Accel_read As Single 'single used to store accelerometer data
Dim Full_power_spin As Integer 'if set to 1 - we're just spinning at full power (no translation)
Dim Alternate_motor_cycle As Integer 'flipped between 1 and 2 each spin - alternates which motor is used for power each cycle when not moving
Dim Forward As Integer '1 if robot is supposed to be going forward
Dim Backward As Integer '1 if robot is supposed to be going back
Dim Begin_brake As Integer 'point in spin to start brake
Dim End_brake As Integer 'point in spin to end brake
Dim Turbo As Integer 'set to 1 if in special superfast mode (less translation)
Dim Periodms As Single 'how long it takes for the robot to rotate once
Dim Delaytime_single As Single 'Delaytime refers to time spent in each "cycle"
Dim Delaytime_long As Long 'Used for actual for / next loops (integer)
Dim In_tracking_adjust As Integer '1 if robot is in tracking adjustment mode
Dim Led_shift As Long 'offset in milliseconds for LED to come on
Dim Throttle_percent As Integer 'percentage of full throttle (spin rate)
Dim Power_kill_offset As Long 'used for throttling - MS offset in spin at which power is cut
Dim G As Single 'g force the accelerometer is seeing
Dim Rpm As Single 'current RPM's of robot
Dim Add_delay As Single 'used to calculate changes in heading
Dim Digitdif As Single 'used in nasty code to convert periodms into an integer
Dim Rand1 As Integer
Dim Randsingle As Single
Dim Leftright As Integer 'heading RC channel
Dim Forwardback As Integer 'forward/back RC channel
Dim Throttle As Integer 'throttle RC channel
Dim Shutdown As Integer 'if set to 1 - robot goes into safety mode
Dim Serialdata As String * 10
Dim Throttle_hilow As Boolean 'indicate if given RC channel was hi or low on last read
Dim Forwardback_hilow As Boolean
Dim Leftright_hilow As Boolean
Dim Rc_count As Integer 'count number of spins since last setting of throttle data (used for safety)
Enable Interrupts
Enable Pcint0
On Pcint0 Rc_change 'call RC_change anytime RC pins go up or down
Pcmsk0 = &B00011001 'sets mask so that only RC pins trigger interrupt
'Setup timers for RC READ
Config Timer0 = Timer , Prescale = 256 'forward / back
Config Timer1 = Timer , Prescale = 64 'timer1 used for left/right - provides higher resolution
Config Timer2 = Timer , Prescale = 256 'throttle
Start Timer0 'start timers for reading RC
Start Timer1
Start Timer2
Disable Timer0 'disabling timer overflow interrupts (may or may not be needed)
Disable Timer1
Disable Timer2
Dim Tracking_comp_store As Eram Single 'used to store tracking adjustment in ROM
Dim Tracking_comp_check As Eram Integer 'used to store validate stored ROM value
Dim Tracking_comp As Single 'user compensation for tracking error
Dim Eprom_single_read As Single 'used to read from eprom
Const Heading_center = 469 'center value for heading
Const Heading_leftthresh = 465
Const Heading_rightthresh = 473
Const Min_rpm = 700 'minimum RPM for translation / throttling to kick in
Const Radius = 2.2915 'effective radius of circle for accel (inches) - seems to be off sometimes...
Const G_per_adc_increment = .5 '10mv / g, 5mv per single increment up to 1024
Const Base_accel = 504.5 'ADC value for accel with no motion
Const Forward_comp = 1.00 'heading compensation when going forward
Const Backward_comp = .99 'heading compensation when going back
Declare Sub Motors_off 'motors off
Declare Sub Motors_left 'both motors on
Declare Sub Motor1_on 'turn motor 1 on
Declare Sub Motor2_on 'turn motor 2 on
A = 1 'value set to be always "true"
Tracking_comp = 1 'tracking compensation defaults to 1 (no adjustment)
If Tracking_comp_check = 555 Then Tracking_comp = Tracking_comp_store 'get tracking_comp from ROM only if Tracking_comp_check was set to 555
Rc_count = 0 'make sure rc_count is 0...
Start Adc 'start ADC for accelerometer
While A = 1 'main loop
Rc_count = Rc_count + 1 'increment RC_count to check for safety (is reset to 0 each time throttle is succesfully received from RC)
'if no rc for 15 spins then shutdown (set throttle to 0)
If Rc_count > 15 Then
Throttle = 0
End If
'if throttle is lower than 90 bot stays powered down
While Throttle < 90 Or Throttle > 200 Or Shutdown = 1
Motors_off
'sit there and flash LED
Toggle Portd.6
Waitms 100
'if the tracking compensation has been changed by the driver - write it out to Eprom
Eprom_single_read = Tracking_comp_store 'reads tracking_comp from eprom
If Tracking_comp <> Eprom_single_read Then 'write out only if changed from last time - otherwise will kill eeprom
Tracking_comp_store = Tracking_comp 'write out current tracking compensation - done here since writting to ROM takes time...
Tracking_comp_check = 555 'write out arbitrary value to validate tracking_comp was written out
End If
'debug data
Serialdata = Str(leftright)
Print Serialdata;
Print ", ";
Serialdata = Str(forwardback)
Print Serialdata;
Print ", ";
Serialdata = Str(throttle)
Print Serialdata;
Print ", ";
Serialdata = Str(tracking_comp)
Print "Tracking_comp:";
Print Serialdata
Serialdata = Str(periodms)
Print "Last periodms:";
Print Serialdata
Print
Wend
Disable Interrupts 'bad things seem to happen if the RC interrupts get triggered while doing math...
'Are we going forward or backwards?
If Forwardback > 120 And Forwardback < 250 Then Forward = 1 Else Forward = 0
If Forwardback < 105 And Forwardback > 50 Then Backward = 1 Else Backward = 0
Accel_raw_data = Getadc(5) 'get accel data (word)
Accel_read = Accel_raw_data 'move it over to single in case we want to do floating point
Accel_read = Accel_read - Base_accel 'compensate for base (2.5v) level
G = Accel_read * G_per_adc_increment 'convert to G's
Rpm = 28.45 * Radius 'calculate RPM from G's - rpm = (G/(28.45* radius ))^0.5 *1000
Rpm = G / Rpm
Rpm = Rpm ^ .5
Rpm = Rpm * 1000
Periodms = Rpm / 60 'convert RPM to duration of each spin in milliseconds
Periodms = 1 / Periodms
Periodms = Periodms * 1000
Periodms = Periodms * Tracking_comp 'compensate with user-set tracking adjustment
If Forward = 1 Then Periodms = Periodms * Forward_comp 'extra compensation if going forward
If Backward = 1 Then Periodms = Periodms * Backward_comp 'extra compensation if going backward
Periodms = Periodms - .07 'each accel read = .07 ms
If Alternate_motor_cycle = 1 Then Alternate_motor_cycle = 2 Else Alternate_motor_cycle = 1 'alternates Alternate_motor_cycle - used to balance spin
Delaytime_single = Periodms / 2 'sets period in MS for each half of spin
'converts throttle reading from remote into percentage (Throttle - 87 = low, 115 = middle, 148 = high)
Throttle_percent = Throttle - 80
Throttle_percent = Throttle_percent * 2
If Throttle_percent > 100 Then Throttle_percent = 100 'don't got over 100%
'tracking adjustment - if throttle is between 1/3 and 1/2 - go into tracking adjustment mode (at full normal speed)
'driver moves stick left and right until the bot tracks correctly
'data is written into eprom next time the robot spins down
If Throttle < 100 Then
In_tracking_adjust = 1
Throttle_percent = 100 'full speed
If Leftright < Heading_leftthresh Then
Tracking_comp = Tracking_comp + .004
End If
If Leftright > Heading_rightthresh Then
Tracking_comp = Tracking_comp - .004
If Tracking_comp < .04 Then Tracking_comp = .04 'don't let it get set too low...
End If
If Forwardback < 105 Then
Tracking_comp = 1 'if stick is pulled backward during heading adjustment - reset to 1
Backward = 0
End If
Else
In_tracking_adjust = 0
'normal drive heading change
'this code adds or subtracts a percentage of delaytime based on the heading data from the remote
Add_delay = Heading_center - Leftright
Add_delay = Add_delay * Delaytime_single
Add_delay = Add_delay / 2200
Delaytime_single = Delaytime_single + Add_delay
End If
'nasty code to convert Delaytime_single into Delaytime_long
'randomly adds 1 to delaytime_long a percentage of time proportionate to how close
'the decimal portion of the number is to 1 (1.4 becomes 2 40% of the time)
'this in effect improves the accuracy of tracking / steering (yes, there are better ways to handle this)
Delaytime_long = Delaytime_single
Digitdif = Delaytime_single - Delaytime_long
Digitdif = Digitdif * 100
Rand1 = Rnd(100)
Randsingle = Rand1
If Digitdif > Randsingle Then Delaytime_long = Delaytime_long + 1
'caps on timing if going too slow or fast
If Delaytime_long > 250 Then Delaytime_long = 250
If Delaytime_long < 5 Then Delaytime_long = 5
Power_kill_offset = Throttle_percent * Delaytime_long 'set time in each cycle to cut power (throttling)
Power_kill_offset = Power_kill_offset / 100
'Do translation ("braking") for full cycle
Begin_brake = 1
End_brake = Delaytime_long
Led_shift = Power_kill_offset / 4 'set relative LED location (depends on time which power is cut)
Led_shift = Led_shift * 3
Turbo = 0
'if we're at top speed ("Turbo") - only do translation for 3/4 of each cycle (start 1/8th late / end 1/8th early)
If Throttle > 136 Then
Turbo = 1
Begin_brake = Delaytime_long / 8
End_brake = Delaytime_long / 8
End_brake = End_brake * 7
Led_shift = Power_kill_offset / 16 'set relative LED location (slightly different for turbo mode)
Led_shift = Led_shift * 10
End If
Full_power_spin = 0
If Rpm < Min_rpm Then Full_power_spin = 1 'if we're under the minimum RPM for translation - do the full power spin!
'special ultra-high-speed mode - does full_power_spin every other cycle if throttle is set to max
'reduced translation - robot tends to drift forward on its own since motors aren't being switched
'if it goes backwards change alternate_motor_cycle to 1
If Throttle > 146 And Alternate_motor_cycle = 2 Then Full_power_spin = 1
Enable Interrupts 'out of all the critical stuff
'if full_power_spin is 1 - just spin at full power - no translation
If Full_power_spin = 1 Then
Motors_left 'full power!
For X = 1 To Delaytime_long
If X > Led_shift Then Portd.6 = 0 'turn off heading led
Waitms 1
Next X
For X = 1 To Delaytime_long
If X > Led_shift Then Portd.6 = 1 'turn on heading led
Waitms 1
Next X
Else
'Do translational drift driving
'Cycle 1 (front 180 degrees of spin)
Portd.1 = 1 'led always on at beginning of first cycle
Motors_left 'start off under full power
For X = 1 To Delaytime_long 'each loop is 1ms (delaytime is length of 180 degrees of cycle)
If X = Begin_brake Then 'switch to single motor as soon as entering braking cycle
'if sitting still
If Alternate_motor_cycle = 1 Then Motor1_on 'alternates which motor is used each cycle if sitting still
If Alternate_motor_cycle = 2 Then Motor2_on 'this prevents unwanted "translation" due to any imbalances
'if going forward / back set motors appropriately (this is "where it happens")
If Forward = 1 Then Motor1_on
If Backward = 1 Then Motor2_on
End If
If X = End_brake Then Motors_left 'if we hit end of brake cycle - go to full power
If X > Power_kill_offset Then Motors_off 'if throttle is less that 100% - kill throttle at appropriate time
'flash heading LED when in turbo mode or tracking adjust
If In_tracking_adjust = 1 Or Turbo = 1 And X < Led_shift Then Toggle Portd.6
If X = Led_shift Then Portd.6 = 0 'turn off heading led at defined time
Waitms 1 'wait 1ms
Next X
'Cycle 2 (back 180 degrees of spin) - pretty much everything works the same...
Portd.6 = 0 'led always off at beginning of second cycle
Motors_left 'start off under full power
For X = 1 To Delaytime_long 'each loop is 1ms (delaytime is length of 180 degrees of cycle)
If X = Begin_brake Then 'switch to single motor as soon as entering braking cycle
'if sitting still
If Alternate_motor_cycle = 1 Then Motor2_on 'alternates which motor is used each cycle if sitting still
If Alternate_motor_cycle = 2 Then Motor1_on 'this prevents unwanted "translation" due to any imbalances
'if going forward / back set motors appropriately (this is "where it happens")
If Forward = 1 Then Motor2_on
If Backward = 1 Then Motor1_on
End If
If X = End_brake Then Motors_left 'if we hit end of brake cycle - go to full power
If X > Power_kill_offset Then Motors_off 'if throttle is less that 100% - kill throttle at appropriate time
'flash heading LED when in turbo mode or tracking adjust
If In_tracking_adjust = 1 Or Turbo = 1 And X > Led_shift Then Toggle Portd.6
If X = Led_shift Then Portd.6 = 1 'turn on heading led as defined time
Waitms 1 'wait 1ms
Next
End If
Wend
Sub Motors_off
Portd.3 = 0
Portd.4 = 0
End Sub
Sub Motors_left
Portd.3 = 1
Portd.4 = 1
End Sub
Sub Motor1_on
Portd.3 = 0
Portd.4 = 1
End Sub
Sub Motor2_on
Portd.3 = 1
Portd.4 = 0
End Sub
'Reads RC data - triggered by RCINT anytime one of the RC pins goes high or low
'Uses timers to determine how long since the signal went high
Rc_change:
If Pinb.4 <> Forwardback_hilow Then
If Pinb.4 = 0 Then 'did the pin go low? - then set timer value as value for this channel...
Forwardback = Timer0
End If
If Pinb.4 = 1 Then 'did the pin go high? - then reset timer...
Timer0 = 0
End If
End If
If Pinb.3 <> Leftright_hilow Then
If Pinb.3 = 0 Then
If Timer1 < 650 Then 'only set if within bounds
If Timer1 > 300 Then
Leftright = Timer1
End If
End If
End If
If Pinb.3 = 1 Then
Timer1 = 0
End If
End If
If Pinb.0 <> Throttle_hilow Then
If Pinb.0 = 0 Then
If Timer2 < 200 Then 'only set if within bounds
If Timer2 > 50 Then
Throttle = Timer2
Rc_count = 0 'got throttle data - reset rc_count
End If
End If
End If
If Pinb.0 = 1 Then
Timer2 = 0
End If
End If
Throttle_hilow = Pinb.0 'make note of all pin states for reference next time interrupt is triggered...
Forwardback_hilow = Pinb.4
Leftright_hilow = Pinb.3
Return
|