#!/usr/local/bin/perl -w
#
#
# This Perl script generates throttle tables (and associated code)
# for the 12F675 based ESC.
#
# SCCS: @(#) throttle.pl 1.9@(#)
#
# Written by: Tim Wooller (tugboat@designsoft.com.au)
#
# Please report any problems to the author.

#
# Note: All constants are in instruction cycles (ie. 1usec)

# Important constants. You can alter these to get different code as
# desired.
#
#	$mapthrottle	Selects your custom throttlemap - the routine
#			throttlemap() is called with the throttle step [0..max],
#			the maximum throttle value,
#			pwm amount [0-1] and the $mapthrottle value.
#			This routine must return the desired mapped PWM
#			amount [0-1].
#
#			Standard values of $mapthrottle are:
#			0 - raw linear (no map), zero throttle is brake
#			1 - linear with additional idle values, idle is brake
#			2 - throttle -> power (ie. %age :: VA), idle is brake
#			3 - car use (throttle->power, brake-idle-run)
#
#			Note: returned PWM ratios < 0 will be taken as brake on,
#			and those < 0.0005 will be take to be off,
#			and those > 0.9995 will be taken to be full on.
#
#	$roundscale	applies rounding, rather then trunctation to
#			prescaler calculations
#
my($mapthrottle)	= 1;
my($roundscale)		= 1;

#
# Special configuration items:
# DO NOT ALTER THESE
my($throttle_calidle)	= 0;

#
# Important note: a return value less that zero will be taken as a braking
# position, while [0,0.0005) will be taken as motor off with no braking.
# (This assumes that the ESC is assembled with the brake enabled, if the
# brake is not build into the ESC then the brake and idle positions will
# just all be motor idle.
#
sub throttlemap
{
    my($throttle, $throttlemax, $pwm, $map) = @_;

    # A standard mapping, use the first 3 throttle positions (0,1,2) as
    # braking because these values don't spin the motor anyway.
    if ($map == 1)
    {
	return -1 if ($throttle <= 2);	# 0,1,2 are brake
	return $pwm;			# all other are linear
    }

    # A standard mapping, uses only one idle position and then maps the
    # throttle to power (ie. VA) delivered to the motor
    if ($map == 2)
    {
	$pwm = sqrt($pwm);
	return -1 if ($pwm < 0.0005);	# Brake at low throttle
	return $pwm;
    }

    # This is a car use map. There is braking at low throttle, then
    # a period of 'idle' and then the normal motor control.
    if ($map == 3)
    {
	# Auto calibrate idle position is mapped to throttle 3
	$throttle_calidle = 3;

	# Now determine the PWM map
	return -1 if ($throttle <= 0);	# 0 is brake
	return 0 if ($throttle <= 5);	# 1,2,3,4,5 are motor off
	return sqrt(($throttle-5)/($throttlemax-5));	# power curve
    }

    # The default is to be linear
    return -1 if ($pwm < 0.0005);	# Brake at low throttle
    return $pwm;
}

# Semi-important constants. You shouldn't alter these unless you know
# what you are doing.
#
#	$min_motor_off	is the minimum time that the motor is permitted
#			to be off during PWM operation. If this value
#			is too long the motor will operate in 'discontinuous'
#			mode and will 'buzz' and vibrate.
#
#	$throttle_steps	the total number of throttle steps used by
#			the PIC software. These are assumed to be in the
#			range 0..$throttle_steps-1. With 0 -> motor off
#			and $throttle_steps-1 -> motor on.
#
#	$minfrequency	The lowest permitted PWM frequency. Normally this
#			should be zero, the software will only generate
#			low frequencies when FET off times are short and
#			these low frequencies are good for efficiency.
#
my($min_motor_off)	= 128;
my($throttle_steps)	= 64;
my($minfrequency)	= 0;

# Constants that relate to the implementation, don't touch these....
#
#	$instruction_clock the instruction clock rate cycles per second
#
#	$timer0_prescale the prescaler in use on timer0
#
#	$max_delay	the largest delay possible
#
#	$int_in_ohead	time from T0INT until desired code is running
#	$int_out_ohead	time from TMR0 running again and next int being
#			possible
#	$int_tm0_ohead	time to rearm TMR0 (note stopped for 2 clks)
#
#	$int_std_pre	time taken in standard interrupt routine prior to
#			the bit change
#	$int_std_post	time taken in standard interrupt routine after the
#			the bit change
#
#	$int_spc_pre	time taken in special routine prior to bit change
#	$int_spc_mid	time taken to restore the bit
#	$int_spc_post	time taken in special routine prior to bit restore
#
my($instruction_clock)	= 1000000;
my($timer0_prescale)	= 2;
my($max_delay)		= 256 * $timer0_prescale;

my($int_in_ohead)	= 8;
my($int_out_ohead)	= 7;
my($int_tm0_ohead)	= 2;
my($int_std_pre)	= 5;
my($int_std_post)	= 3;
my($int_spc_pre)	= 2;
my($int_spc_mid)	= 2;
my($int_spc_post)	= 2 + 1 + 2;

# Compute the minimum permited clock delay for standard PWM processing,
# and the various offsets
my($min_int_std)	= $int_in_ohead + $int_std_pre
			+ $int_std_post + $int_tm0_ohead
			+ $int_out_ohead;

#
# Our local variables.
my($throttle_nonidle);	# Lowest non-idle throttle setting

my($context);		# A context message for assistance in errors

my(@onoff);		# Desired on/off times in clocks
my(@fonoff);		# Final on/off times in clocks

my(@errors);		# Errors that we encountered
my(@doccode);		# Documentation code
my(@intcode);		# Interrupt code we need to generate
my(@pwmtype);		# Code to determine pwmtype
my(@pwmON);		# Code to determine pwmONt0
my(@pwmOFF);		# Code to determine pwmOFFt0

my(%codeok);		# Which code fragments already exist
my(@dlyofflbl);		# Code definitions for delay then clrf GPIO
my(@dlyonlbl);		# Code definitions for delay then set GPIO

my(@aligncode) =	# Code to align the computed goto tables
(
    "; Ensure correct alignment of this table for a computed goto",
    "\tif (\$ & ~0FFh) != ((\$+$throttle_steps-1) & ~0FFh)",
    "\t\torg (\$+$throttle_steps) & ~0FFh",
    "\tendif"
);

#
# Special code to generate delay loops for the PIC
sub getdelay
{
    my($cnt) = @_;
    my(@rslt);

    push @rslt, "; Delay $cnt";
    while ($cnt >= 4)
    {
	$cnt -= 4;
	push @rslt, "\tcall\temptyreturn";
    }
    while ($cnt >= 2)
    {
	$cnt -= 2;
	push @rslt, "\tgoto\t\$+1";
    }
    while ($cnt > 0)
    {
	$cnt -= 1;
	push @rslt, "\tnop";
    }

    return @rslt;
}

#
# Convert the supplied clock count into a 'prescaler' compatible value,
# Values of zero are not permitted because the counter interrupts on
# 0xFF -> 0x00 transition and so 0 will be considered as 256 for timing
# purposes...
sub applyprescale
{
    my($count) = @_;

    # Try rounding rather than truncation in an attempt to minimise error
    $count += $timer0_prescale / 2
	if ($roundscale);

    # And apply the prescaler
    $count = int($count / $timer0_prescale);

    # And don't allow zero.......
    if ($count <= 0)
    {
	push @errors, "; WARNING at: $context";
	push @errors, "; Timer prescale produced $count. Value of 1 used.";

	$count = 1;
    }

    # Don't allow counts that are too large
    if ($count > 256)
    {
	push @errors, "; WARNING at: $context";
	push @errors, "; Timer prescale produced $count. Value of 256 used.";

	$count = 256;
    }

    return $count;
}

#
# Now, step one is to generate the motor PWM rates that are required
my($th);
for ($th = 0; $th < $throttle_steps; ++$th)
{
    # Calculate the desired PWM on percentage, and adjust it if we are in
    # mapthrottle mode...
    my($pwm);

    $pwm = $th/($throttle_steps-1);
    $pwm = throttlemap($th, $throttle_steps-1, $pwm, $mapthrottle)
	if ($mapthrottle != 0);

    # Code the brake, zero and full throttle cases specially
    if ($pwm < 0)
    {
	$onoff[$th] = [0, 0, $max_delay, 1];
	next;
    }

    if ($pwm <= 0.0005)
    {
	$onoff[$th] = [0, 0, $max_delay, 0];
	next;
    }

    $throttle_nonidle = $th
	if (!defined($throttle_nonidle));

    if ($pwm >= 0.9995)
    {
	$onoff[$th] = [1, $max_delay, 0, 0];
	next;
    }

    #
    # Now determine the on and off clocks that we want to use, the basic
    # idea is to go with the lowest frequency PWM that gives us an acceptable
    # off time...
    my($on, $off);

    # Estimate using the minimum motor off time
    $off = $min_motor_off;
    $on = int(($off / (1-$pwm)) * $pwm);

    # Now adjust for the limits of our counters
    if ($on > $max_delay)
    {
	$on = $max_delay;
	$off = int(($on / $pwm) * (1-$pwm));
    }

    # And possibly limit if the frequency is too low
    if ($instruction_clock/($on + $off) < $minfrequency)
    {
	$on = int($on * ($instruction_clock/($on + $off)) / $minfrequency);
	$off = int(($on / $pwm) * (1-$pwm));
    }

    # Remember this
    $onoff[$th] = [$pwm, $on, $off, 0];
}

#
# And some documentation, including the key variables for later reference
push @doccode, "; Generated: " . scalar localtime();
push @doccode, "; Using: $0";
push @doccode, "; Version: 1.9";
push @doccode, ";";
push @doccode, "; **** DO NOT EDIT THIS FILE BY HAND ****";
push @doccode, "; **** THIS IS COMPUTER GENERATED CODE ****";
push @doccode, ";";
push @doccode, sprintf("; %30s = %s", 'mapthrottle', $mapthrottle);
push @doccode, sprintf("; %30s = %s", 'roundscale', $roundscale);
push @doccode, ";";
push @doccode, sprintf("; %30s = %s", 'throttle_calidle', $throttle_calidle);
push @doccode, ";";
push @doccode, sprintf("; %30s = %s", 'min_motor_off', $min_motor_off);
push @doccode, sprintf("; %30s = %s", 'throttle_steps', $throttle_steps);
push @doccode, sprintf("; %30s = %s", 'minfrequency', $minfrequency);
push @doccode, ";";
push @doccode, sprintf("; %30s = %s", 'instruction_clock', $instruction_clock);
push @doccode, sprintf("; %30s = %s", 'timer0_prescale', $timer0_prescale);
push @doccode, sprintf("; %30s = %s", 'max_delay', $max_delay);
push @doccode, ";";
push @doccode, sprintf("; %30s = %s", 'int_in_ohead', $int_in_ohead);
push @doccode, sprintf("; %30s = %s", 'int_out_ohead', $int_out_ohead);
push @doccode, sprintf("; %30s = %s", 'int_tm0_ohead', $int_tm0_ohead);
push @doccode, sprintf("; %30s = %s", 'int_std_pre', $int_std_pre);
push @doccode, sprintf("; %30s = %s", 'int_std_post', $int_std_post);
push @doccode, sprintf("; %30s = %s", 'int_spc_pre', $int_spc_pre);
push @doccode, sprintf("; %30s = %s", 'int_spc_mid', $int_spc_mid);
push @doccode, sprintf("; %30s = %s", 'int_spc_post', $int_spc_post);
push @doccode, sprintf("; %30s = %s", 'min_int_std', $min_int_std);
push @doccode, ";";
push @doccode, '; Trottle     Mapped  ---- Target PWM -----    ---- Actual PWM ----    ERROR';
push @doccode, ';  #:%age     %age    on:off  PWM Details      on:off  PWM Details';

# Also generate some initial definitions
push @intcode, "throttlesteps\tequ\t$throttle_steps"
    if (defined($throttle_steps));
push @intcode, "throttlenonidle\tequ\t$throttle_nonidle"
    if (defined($throttle_nonidle));
push @intcode, "throttlecalidle\tequ\t$throttle_calidle"
    if (defined($throttle_calidle));

#
# Now process the onoff array and convert into appropriate structures.
# For the PIC 12F675 the structures are really code that initialises things
# and/or returns values.

# Step 1: Process the data and check for all the PWM settings where the
#         on time is too small for standard interrupt processing.
#	  For the zero on-time settings we need to know if whether
#	  we need the brake and nobrake versions of the code.
my(@onfast);
my(@offfast);
my($zerobrake, $zeroidle) = (0,0);
for ($th = 0; $th < $throttle_steps; ++$th)
{
    my($pwm, $on, $off, $brake) = @{$onoff[$th]};	# Target values

    $onfast[$on] = 1
	if ($on < $min_int_std);		# Special code required
    $offfast[$off] = 1
	if ($off < $min_int_std);		# Special code required

    #
    # For the zero ontime case check for both a brake and no brake case
    if ($on == 0)
    {
	$zerobrake = 1 if ($brake);
	$zeroidle = 1 if (!$brake);
    }
}

# Step 2: Generate this code in the 'correct' order so that the maximum
# 	  amount of timing code can be shared. The entire PWM engine
#	  has to fit into 256 bytes (one page).
#	  - so we go low to high on the on code
#	  - then low to high on the off code
for ($th = 0; $th <= $#onfast; ++$th)
{
    my($on) = $th;

    next if (!defined($onfast[$th]));		# Skip unwanted cases

    # The on time short, so we need to use special on processing
    my($itype) = "int_${on}on";

    # Generate the code
    if ($on >= $int_spc_mid - 1)
    {
	# Generate the code to turn the signal on
	push @intcode, $itype;
	push @intcode, "\tmovfw\tpwmONio";
	push @intcode, "\tmovwf\tGPIO";		# Turn bit ON

	# Now optimise the off code to be as small as possible
	# this involves computing as much reused code as possible
	my($dly) = $on - $int_spc_mid;
	push @intcode, "int_on${dly}off";
	$dlyofflbl[$dly] = 1;

	# Find the closest existing delay.....
	my($found);
	for ($found = $dly-2; $found > 0; --$found)
	{
	    last if (defined($dlyofflbl[$found]));
	}

	# Did we find a delay?
	if ($found > 0)
	{
	    if ($dly - $found - 2 > 0)
	    {
		push @intcode, getdelay($dly - $found - 2);

		push @intcode, "int_on" . ($found+2) . "off";
		$dlyofflbl[$found + 2] = 1;
	    }

	    push @intcode, "\tgoto\tint_on${found}off";
	}
	else
	{
	    push @intcode, getdelay($dly);
	    push @intcode, "\tmovfw\tpwmOFFio";
	    push @intcode, "\tmovwf\tGPIO";		# Turn bit OFF
	    push @intcode, "\tgoto\trearm_OFFt0";
	}

	# Record the ontime achieved
	$codeok{$itype} = $on;
    }
    else
    {
	# Current PIC code requires both the idle and brake
	# code to be present so ignore $zerobrake/$zeroidle
	# 
	# Do we need a zero on time brake case?
	if ($on == 0)
	{
	    # Generate the code
	    push @intcode, "int_${on}brake";
	    push @intcode, "\tmovfw\tpwmOFFio";
	    push @intcode, "\tmovwf\tGPIO";		# Turn bit OFF
	    push @intcode, getdelay($on);
	    push @intcode, "\tgoto\trearm_OFFt0";

	    # Record the ontime achieved
	    $codeok{"int_${on}brake"} = 0;
	}

	# Generate the code for non-zero on times, or zero idles
	{
	    push @intcode, $itype;
	    push @intcode, "\tmovfw\tpwmOFFio";
	    push @intcode, "\tmovwf\tGPIO";		# Turn bit OFF
	    push @intcode, getdelay($on);
	    push @intcode, "\tgoto\trearm_OFFt0";

	    # Record the ontime achieved
	    $codeok{$itype} = 0;
	}
    }
}

for ($th = 0; $th <= $#offfast; ++$th)
{
    my($off) = $th;

    next if (!defined($offfast[$th]));		# Skip unwanted cases

    # The off time short, so we need to use special on processing
    my($itype) = "int_${off}off";

    # Generate some code
    push @intcode, $itype;
    if ($off >= $int_spc_mid)
    {
	# Generate code
	push @intcode, "\tmovfw\tpwmOFFio";
	push @intcode, "\tmovwf\tGPIO";		# Turn bit OFF

	# Now optimise the on code to be as small as possible
	# this involves computing as much reused code as possible
	my($dly) = $off - $int_spc_mid;
	push @intcode, "int_off${dly}on";
	$dlyonlbl[$dly] = 1;

	# Find the closest existing delay.....
	my($found);
	for ($found = $dly-2; $found > 0; --$found)
	{
	    last if (defined($dlyonlbl[$found]));
	}

	# Did we find a delay?
	if ($found > 0)
	{
	    if ($dly - $found - 2 > 0)
	    {
		push @intcode, getdelay($dly - $found - 2);

		push @intcode, "int_off" . ($found+2) . "on";
		$dlyonlbl[$found + 2] = 1;
	    }

	    push @intcode, "\tgoto\tint_off${found}on";
	}
	else
	{
	    push @intcode, getdelay($dly);
	    push @intcode, "\tmovfw\tpwmONio";
	    push @intcode, "\tmovwf\tGPIO";		# Turn bit ON
	    push @intcode, "\tgoto\trearm_ONt0";
	}

	# Save the off time
	$codeok{$itype} = $off;
    }
    else
    {
	# Generate code
	push @intcode, "\tmovfw\tpwmONio";
	push @intcode, "\tmovwf\tGPIO";		# Turn bit ON
	push @intcode, getdelay($off);
	push @intcode, "\tgoto\trearm_ONt0";

	# Save the off time
	$codeok{$itype} = 0;
    }
}


# Step 3: Generate the tables etc that use the code
for ($t = 0; $t < $throttle_steps; ++$t)
{
    my($pwm, $on, $off, $brake) = @{$onoff[$t]};# Target values
    my($fon, $foff);				# Final achieved values

    # Where are we?
    $context = "Trotttle setting $t";

    # Save the PWM parameters that apply here
    push @doccode, sprintf(";%s%2d:%.3f -> %.3f%s %3d:%3d  %.3f @ %4dHz",
			    ($t == $throttle_calidle? '>': ' '),
			    $t, $t/$throttle_steps, $pwm,
			    ($brake? 'B': ' '),
			    $on, $off, $on / ($on + $off),
			    $instruction_clock/($on + $off));
    
    # Now consider the type of interrupt to perform. Note that due to the
    # way these values are constructed only one of them can be vary small
    # and hence require special interrupt processing code. Also pwmON always
    # refers to the FET on phase and pwmOFF is the FET off phase.

    my($itype);

    # Consider short 'on' times, these are ones that are so small that
    # standard interrupt processing is not possible
    if ($on < $min_int_std)
    {
	# The on time is too short, so we need to use special on processing
	$itype = $brake? "int_${on}brake": "int_${on}on";

	# and adjust the times
	$on = 0;
	$off -= $int_spc_post + $int_tm0_ohead + $int_in_ohead + $int_spc_pre;
	$off = applyprescale($off);

	# Compute final time actually achieved
	$fon = $codeok{$itype};
	$foff = ($off * $timer0_prescale) + $int_spc_post
		+ $int_tm0_ohead + $int_in_ohead + $int_spc_pre;
    }
    elsif ($off < $min_int_std)
    {
	# The off time is too short, so we need to use special off processing
	$itype = "int_${off}off";

	# Adjust the times
	$off = 0;
	$on -= $int_spc_post + $int_tm0_ohead + $int_in_ohead + $int_spc_pre;
	$on = applyprescale($on);

	# Record the actual times achieved
	$fon = ($on * $timer0_prescale) + $int_spc_post
		+ $int_tm0_ohead + $int_in_ohead + $int_spc_pre;
	$foff = $codeok{$itype};
    }
    else
    {
	# Everything is 'normal' so we can use standard processing
	$itype = 'int_std';

	# Adjust for the time that TM0 is not running
	$on -= $int_std_post + $int_tm0_ohead + $int_in_ohead + $int_std_pre;
	$off -= $int_std_post + $int_tm0_ohead + $int_in_ohead + $int_std_pre;

	# Then the prescaler
	$on = applyprescale($on);
	$off = applyprescale($off);

	# Compute the final run times
	$fon = ($on * $timer0_prescale) + $int_std_post
		+ $int_tm0_ohead + $int_in_ohead + $int_std_pre;
	$foff = ($off * $timer0_prescale) + $int_std_post
		+ $int_tm0_ohead + $int_in_ohead + $int_std_pre;
    }

    # Save the lookup tables
    $pwmtype[$t] = "\tretlw\t$itype - int_refpoint";
    $pwmON[$t] = "\tretlw\t-$on";
    $pwmOFF[$t] = "\tretlw\t-$off";

    # Record the actual PWM information...... (for docn. purposes)
    #
    # Compute the error in throttle on time as a percentage from what was
    # desired.
    my($error);

    # Error
    $error = $fon/($fon + $foff) - $pwm;
    
    # Convert to a %age, avoid div zero.....
    if ($error != 0)
    {
	$error = 100 * $error / $pwm;
    }

    $doccode[$#doccode] .= sprintf("  %3d:%3d %.3f @ %4dHz  (%5.2f%%)",
			    $fon, $foff, $fon / ($fon + $foff),
			    $instruction_clock/($fon + $foff),
			    $error);
}

#
# And then produce the code for the interrupt handler
print join("\n", @doccode), "\n";
print join("\n", @intcode), "\n";

print join("\n", @aligncode, 'pwmtype_tbl', @pwmtype), "\n";
print join("\n", @aligncode, 'pwmONt0_tbl', @pwmON), "\n";
print join("\n", @aligncode, 'pwmOFFt0_tbl', @pwmOFF), "\n";