Set Velocities
Description
Once the forward and rotational velocity commands have been received and decoded, they need to be transformed into a pair of differential duty cycles for the left and right wheels. There are four numbers involved here, each of which is an integer between -100 and 100: the commanded forward velocity, the commanded rotational velocity, the duty cycle on the left wheel, and the duty cycle on the right wheel. (In actual implementation, the duty cycles only range from 0 to 100; however, using the other motor input as a direction bit, we can effectively vary the duty cycle from -100 to 100, and it is easier to think of it like that for the purposes of describing the algorithms.) Let’s call these four quantities FV, RV, LW, and RW, respectively.
At first glance, the logical formula to implement seems to be the following: if RV >= 0 (so straight ahead or left turn), RW = FV, LW = FV*(1 – RV/50); if RV < 0 (right turn), LW = FV, RW = FV*(1 + RV/50). This way, zero rotational velocity always results in both wheels running at the forward velocity duty cycle, while 100% rotational velocity always results in both wheels having the same magnitude duty cycle (again, the forward velocity magnitude), but being opposite in sign, for maximum turning capability.
Unfortunately, however, this formula involves both non-trivial multiplications and divisions, making it extremely difficult and time-inefficient to implement in assembly (it would be just as time-inefficient if implemented in C, but at least the programmer wouldn’t notice.) Is there a significantly simpler set of formulas that result in the same behavior? In fact, there is! If FV >=0, LW = (FV–RV)/2, RW = (FV+RV)/2; if FV < 0, LW = (FV+RV)/2, RW = (FV–RV)/2. These formulas are much faster to calculate and easier to implement, since the division by 2 can be implemented using a bit-shift, and produce almost identical behavior. 100% rotational velocity still results in equal-magnitude-opposite-sign duty cycles if forward velocity is zero; if not, this property does not hold but more rotational velocity still results in a greater differential of duty cycles between the two wheels; the fact that some forward velocity component still remains may, in fact, be closer to the intent of the command. In general, these formulas always satisfy the property that LW + RW = FV, but make LW and RW unequal to effect turning. One downside of this is that full speed ahead with zero turning results in 50% duty cycle on each wheel rather than 100%; however, this is actually desirable given how fast the Maxon motors try to turn the giant waterwheels at 100% duty cycle.
Implementation of these formulas is pretty straightforward in the assembly; the only tricky part is that the carry bit needs to be explicitly manipulated before performing the bit shift in order to ensure that the result has the same sign as the operand. Also, the formulas are actually implemented FV/2 + RV/2 and FV/2 – RV/2, so as to prevent the addition of two 100% duty cycles from overflowing the 8-bit register (since the values are being interpreted as signed, only 7-bits are available for magnitude.)
Pseudo-Code
Divide Forward Velocity by 2, properly sign-extending to keep the same sign
Divide Rotational Velocity by 2, properly sign-extending to keep the same sign
Subtract RV/2 from FV/2, and save the result as SUB
Add RV/2 to FV/2, and save the result as ADD
If Forward Velocity is greater than or equal to zero
Send SUB to the left wheel over SPI
Send it again to make the slave select timing work
Send ADD to the right wheel over SPI
Send it again to make the slave select timing work
Otherwise (Forward Velocity is less than zero)
Send ADD to the left wheel over SPI
Send it again to make the slave select timing work
Send SUB to the right wheel over SPI
Send it again to make the slave select timing work
Lower both slave select lines to ensure we don’t send garbage after we leave this function
PWM Module
Description
The PWM module on the PICs mostly serves to simply set up the PWM subsystem as required by the PIC architecture, varying the duty cycle register when asked by the main state machine. A little cleverness is employed, however, in order to make the calculation of the value to put into the duty cycle register as easy as possible.
The duty cycle register on the PIC PWM subsystem does not take an explicit duty cycle between 0 and 100; rather, it takes a number to count up to for the active time of the signal, and then continues counting until it reaches the number in the period register before starting the next cycle. Thus, the number that needs to be placed in the duty cycle register is (duty cycle)*(period register + 1)/100. We are again faced with the need to perform an arbitrary multiplication and division, which is difficult and inefficient in assembly. Certain choices of value for the period register can make this calculation much simpler (the obvious choice being period register + 1 = 100), but its value is also limited by the need to run the driven devices at particular frequencies.
The Maxon motors driving the waterwheels can function over a very wide range of frequencies; for various reasons, however, they were set to a frequency of 500 Hz for the ME 218B project, and so it was desirable to keep them in that neighborhood of frequencies for this project as well. The servo motor (used to actuate the ball-catching net that was not, in the end, implemented on the TOWRP), only worked with a very narrow band of frequencies, approximately 500 Hz to 650 Hz. Notably, both driven devices were able to operate at 625 Hz, a value which did indeed simplify the duty cycle calculation.
The slave PICs driving the Maxon motors were running off of a 4 MHz internal oscillator; thus, a PWM frequency of 625 Hz was achieved by setting period register + 1 = 100. Therefore, the duty cycle calculation was simplified to duty cycle register = duty cycle. The master PIC driving the servo motor was running off of a 10 MHz external oscillator; for that, a 625 Hz frequency was achieved when period register + 1 = 250. This means that the duty cycle calculation had to be 2.5*(duty cycle). This was still quite reasonable, however, since breaking that down into 2*(duty cycle) + 0.5*(duty cycle) simply required two bit-shifts to be performed.
Once the frequency values were picked to simplify the duty cycle calculation, the rest of the work of the PWM module was mostly trivial. A few other interesting features include the ability to handle the receipt of a negative duty cycle (by toggling the direction bit and then using the commanded duty cycle as an active low PWM signal rather than active high), and the correct handling of overflow in the duty cycle register, either due to the addition in the formula or an invalid command in the first place. For the slave PICs, this entire process is interrupt-driven: the PWM module is called upon receipt of an SPI transmission received interrupt. In that case, the first thing that is done is to get the data byte from the SPI register, and then manipulate it as any other duty cycle command input.
Pseudo-Code
Initialization:
Set the period register to output PWM at 625 Hz
Make the PWM and direction pins outputs that are not analog
Initialize to a zero duty cycle
Use single-output PWM with active high signals
Only use the upper eight bits of the duty cycle register, since we only need whole numbers
Set the TMR2 pre-scaler to successfully make the frequency 625 Hz
Turn on TMR2 to start the PWM
Update Duty Cycle:
If the commanded duty cycle is greater than or equal to zero
Set the direction to be forward
Set the PWM to be active high
Otherwise (commanded duty cycle is less than zero)
Set the direction to be reverse
Set the PWM to be active low
Take the 2’s complement of the commanded duty cycle to make it positive
If this is the master PIC/servo motor, and therefore the formula is 2.5*(duty cycle)
Clear the carry bit and shift the duty cycle left to multiply by 2
Clear the carry bit and shift the duty cycle right to divide by 2
Add these two results together to get 2.5*(duty cycle)
If this addition overflowed the register, replace the result with 250
Move this value into the duty cycle register
Otherwise (this is the slave PIC/drive motor, and therefore the formula is just (duty cycle))
Move this value into the duty cycle register (it’s already ready to go)