Car-Gondola Report

Embedded Control
Sam Ellis Sonike Hendricks Eric Fish Michael Fredericks Section 4 - Side B 12.7.2012

Abstract Purpose and Objectives Overview of Contoller Hardware Description

overview speed controller servo motor compass ranger lcd/keypad accelerometer serial bus pin connections from priority table

3 3 4 5

Software Description
Overview Initialization - Ports, PCA, ADC, XBR0 Use of the PCA Pulse Streams Setting the Crossbar I2C Read and Write Use of I2C (SMB) Reading Analog Input, A/D Results Keypad input & LCD output Control loops Timing

Results & Conclusions

Analysis of results Description of gondola performance What was Learned Problems Encountered & Solution

12

References Participation Code Flowchart Pseudocode Schematic

15 15 16 34 35 36
2

Abstract
At the beginning of this semester long project we introduced ourselves to the SciLabs C8051 microprocessor and created a user interactive game of hexadecimal-binary conversion, call the Hex-Bin Game. Since the completion of this project we have moved on to implementing new microcontroller features like the programmable counter array and serial communications along with C code control algorithms to create embedded control systems for our smart car. Later we will be implementing these control systems templates on the Gondola. The basic building blocks learned in our previous experience with the C8051 microcontroller will also be utilized including timers, interrupts and analog to digital conversion. This report documents the development of a closed loop hardware-software system intending to control the steering and velocity of the smart car based on sensor data. These sensors include an ultrasonic compass, electronic compass and accelerometer. Initially we explored proportional control for our car with addition of derivative control on our Gondola to improve control response. Proportional control Source code was developed to appropriately initialize our microcontroller, read data from the sensors via I2C serial communication and output appropriate pulse width signals to the motors. Implementation of an LCD display and numerical keypad allows for user inputs of heading and control gains. The goals of the system were to achieve a desired heading and velocity based on the range and current heading of the Car. In addition to a desired heading the Gondola was desired altitude based on the range and current heading. Lastly data was collected and plotted for a given desired heading to quantify the performance of the system comparing various gain constants. Specifics towards the objectives and purpose of our project will be discussed next.

Purpose and Objectives Labs

Our Car and Gondola embedded control system can be divided into two main subsystems. The steering and heading subsystem is responsible for steering the car given the heading from our electronic compass. The drive and range subsystem is responsible for controlling the car speed based object distance to our ultrasonic ranger. Our hardware objectives were quite short requiring only the configuration of the steering servo, speed controller, electronic compass, ultrasonic ranger, accelerometer, LCD screen and numeric pad. For our steer and heading subsystem we had to initialize and configure the programmable counter array for pulse width modulation. We then calibrated the servo pulse width for extreme left, extreme right and center positions. A program was developed to control the steering servo via the laptop keypad. Next the I2C serial bus was initialized and configured to read data from the electronic compass indicating the Cars heading. A control algorithm will use the heading to control the pulse width signal of the steering servo. This algorithm will also need to be modified to control the heading of the Gondola. For our drive and range subsystem we had to initialize and configure the programmable counter array for pulse width modulation as before. The speed controller needs to be calibrated for neutral, maximum speed forward and maximum speed in reverse. We also must develop a program to test control of drive motor via the laptop keypad. Next the I2C serial bus is initialized and configured to read data from the ultrasonic ranger indicating the distance above the 3

Car. A control algorithm will use this range to control the velocity of the drive motor. This algorithm will also need to be modified to control the altitude of the Gondola. The microcontroller requires a minimum operation voltage so a voltage divider must be utilized to monitor the battery voltage. An analog-digital conversion must also be initialized and configured to convert this signal for display on the LCD display. The LCD display and numeric pad must also be initialized and configured to receive user inputs and display desired data. After initializing and configuring the accelerometer the x-coordinate and y-coordinate acceleration data would be used to control the steering servo and speed controller. Unusually the x-coordinate acceleration data will control both heading and velocity in our algorithm. Once completing all the control systems for the Car, source code needs to be developed for controlling components of the Gondola; tail fan speed controller, right thrust fan speed controller, left thrust fan speed controller, thrust angle servo, ultrasonic range, and electronic compass. The steering and propulsion subsystems of the Gondola and Car have similar functionality and modifying our source code should also reflect this. A proportional plus derivative control algorithm is to be used for the Gondola to increase dampening of the system, and increase system response. The numeric keypad will allow for input of the desired heading, proportional and derivative gain values for the heading, and proportional and derivative gain values for the altitude. On the LCD the battery voltage, desired head, current heading will be displayed along with all fan pulse widths. Collect data that shows how the gondolas response is affected by the values of the proportional and derivative gains.

Overview of Microcontroller
In this laboratory the Silicon Labs C8051F020 microcontroller has proven its diversity in embedded control applications. A few specifications features include 64k bytes of internal flash and 4k bytes of internal RAM, RS232 and serial communication. There are seven digital input/output ports but we will only be utilizing ports 0, 1, 2 and 3. Each port has eight pins, one bit each, where 0V is logic FALSE and +3V is logic TRUE. The microcontroller is capable of analogdigital conversion on Port 1. It is also capable of serial communication via the I2C Bus, pulse width modulation via five capture and compare modules, and interrupt service routines via up to five timers. Using a SDCC Compiler we will program the microcontroller in C code. Conveniently there are an abundant number of useful special registry functions used to initialize and calibrate the microcontrollers plethora of features.

Hardware Description
Overview Labs three through six introduced many new and complicated hardware components necessary to the completion of the respective labs. First were the speed controller and the servo motor, which were built in to the smart car, but needed to be attached to the microcontroller. The next additions were the compass and the ranger, which needed to be wired to a specific section of the smart car. After, the LCD/keypad was introduced, and wired to the car to provide input and feedback without having to use the serial monitor. Finally, the accelerometer was the last piece of hardware needed to be wired to the car. When the gondola needed to be used, it incorporated all the previous hardware except for the accelerometer, and everything had been previously wired inside the shell. Pin Connections from Priority Table Before going into detail on the description of various hardware components, it is important to understand the hierarchy of pin connections. To comprehend the pin structure, the back of the lab manual provides a Priority Crossbar Decode Table. By using this table appropriately, correct pin connections are ensured. For example, if 4 Capture Compare Modules needed to be used, they would want to occupy port 0 and pins 0 to 3. However, some other registers take precedence over the CCMs. If UART0EN and SPI0EN both need to be enabled, this will offset the CCMs to now take up port 0 (pins 6 and 7) and port 1 (pins 0 and 1). More information about the crossbar will be discussed in the software section. Serial Bus The Serial Bus (also referred to as the IC Bus) is used to control the compass, ranger, accelerometer, and LCD/keypad. For the completed labs, it always used synchronous communication at 100 kHz. The Serial Bus needs 2 wires to run properly, one for transferring data and the other for the clock. There are two entities that connect to the Serial Bus: the master or the slave. The microcontroller will always be the master, whereas the other hardware will be the slaves. For the labs that included the use of any of the hardware mentioned previously, each component needs to be wired to both the data line and the clock line. Also, a set of pull-up resistors is mandatory for the hardware to work properly. Only one set of these resistors is required, but all the hardware must be connected to them. The connection of the Serial Bus to individual components will be discussed later in this section. Speed Controller The Speed Controller is used to drive the back wheels of the smart car from labs 3 to 5. It is driven by pulse width modulation using the programmable counter array. The back wheels are set in motion by varying the pulse width, and able to range from full reverse to full forward. The Drive Motor has a 20 millisecond period, and wheel speed is changed by varying the percentage of time the signal is high. The percentage of time the signal is high in one period is known as the duty cycle. For the smart car, 1.1 milliseconds corresponds to full reverse, 1.5 milliseconds corresponds to 5

neutral, and 1.9 milliseconds corresponds to full forward. These values are then calculated into pulse widths at 2030, 2760, and 3500, respectively. One important fact to note about the Drive Motor is that it should always be initialized to neutral for at least one second upon starting the code. The only connection needed to be made between the microcontroller and the Drive Motor was to run a single wire from the Speed Controller to the corresponding port and pin. Servo Motor The Servo Motor was very similar to the Drive Motor. It operated with a 20 millisecond period, and front wheel orientation was varied in the same manner as back wheel speed. The Servo Motor should also always start in the neutral position, set at a pulse width of around 2693. If the smart car is turning full left, the pulse width gets set to 2143, and the pulse width gets set to 3173 for full right turning. The pulse width varies in between these two extremes to produce a smooth turning for the smart car. Similar to the Drive Motor, the only wiring needed to be done was to run a single wire from the Servo Motor connection to the corresponding port and pin. Both the Servo and Drive Motors had all of the hardware built into the smart car, only leaving the connection to the microcontroller to ensure proper functioning. Ranger The ranger was used to receive feedback on how far away an object was from the smart car. It was initialized to read and output values in centimeters as was necessary for the labs. The ranger sends out a high frequency sound waves at about 0.9 feet per millisecond which reflect off surfaces and bounce back. If the ranger receives an echo, it computes the distance from the time taken for the signals cycle. When the ranger was initialized correctly, it was accurate until less than 3 centimeters, where it started encountering problems. This was not an issue though as the labs never required a reading less than 3 centimeters to be completed. One important aspect about correct operation of the ranger was to ensure it did not send out a signal faster than every 65 milliseconds. If the ranger sent out signals faster, the signals would interfere and cause false outputs. The ranger was plugged into a special proto board located on the side of the smart car, but needed to be wired to the microcontroller. The ranger had 5 pins, 4 of which were connected to ensure a working component. The last pin was used to check the current software iteration on the ranger. The ranger needed to be connected to power, ground, system data, and the system clock. The latter two connections communicated with the serial bus, whereas the former two were necessary to power the ranger. In addition to these 4 connections, a 10 micro-Farad capacitor needed to be connected between the power and ground leads. Also, it was mandatory that the data and clock connections each ran through a respective pull-up resistor. Compass The compass was used to determine the direction of the smart car in reference to magnetic north. When connected correctly, it reads a value between 0 and 3599 determined by the smart cars current orientation. The numbering system relates to 10 times the degree value of the rotation from north. So, for example, if the smart car is 6

heading east, the compass value would read 900, corresponding to 90 degrees. The compass was also connected to the same proto board that the ranger was connected to. Similar to the ranger, the compass also required 4 connections to run properly. The compass needed to be connected to power, ground, system data, and system clock. It is important to note that the compass has a total of 9 pins, but only pins 1, 2, 3, and 9 need to be connected. Finally, the system data and system clock line both need to be brought through the same pull-up resistors that the ranger went through. LCD/Keypad The LCD and Keypad come assembled together as one unit. The LCD has a total capacity to display 80 characters 4 rows of 20 characters each. The keypad contains the numbers 0 through 9, as well as the star (*) and the pound symbol (#). The LCD/Keypad has a very similar setup to the two previous hardware devices. It needs to be connected to power, ground, system data, and system clock. Just like the ranger and compass, the data and clock lines for the LCD/Keypad need to be fed through a set of pull-up resistors. These pull-up resistors should be the same ones as used for the previous two components, therefore, there should only be one set of pull-up resistors used on the microcontroller. Accelerometer The accelerometer is used to determine the current tilt of the smart car. It uses a 3 axis system to be able to determine acceleration on all 3 axes. The z-axis points straight down into the accelerometer. The x- and y-axes are parallel to the plane of the accelerometer, offset by 90 degrees from each other. Unfortunately, the accelerometer being used in the lab is somewhat unreliable and reports a lot of noise. To overcome this issue, multiple readings need to be averaged together to receive a more accurate estimate. This process should be done between 8 and 16 times per reading, and while not a very sophisticated method, it gets the job done appropriately. The accelerometer has a similar wiring pattern to the previous hardware. It has a total of 8 pins, but only 4 are required to run the accelerometer in the manner necessary for the lab. It requires power, ground, system data, and system clock. Also, the data and clock lines need to be fed through the pull up resistors discussed previously.

Software Description
Overview Much of the code for labs 3, 4, 5 and 6 is the same. This is due to the fact that the circuit elements and coding goals are largely the same over all three labs. Input from an electronic compass is used to control the direction of the vehicle for all three labs, with the difference lying in the type of hardware used to control the direction. In labs 3, 4 and 5, the ranger is used to control the servo mounted on the front axle of the car, which controlled the cars direction. In lab 6, this input is used to control the motor on the rear fan of the gondola. This meant that the code could be imported without changes. The code to control the speed of the vehicle changed slightly as the data acquisition device changed. In labs 3, 4 and 6, input was gathered from an ultrasonic ranger, while in lab 5, an accelerometer was used. This led to slight modifications in the pulse width calculations, but left the bulk of the code unchanged. The final coding aspect to this project was implementing the LCD display and keypad. This device served the same function in all three labs. The user had to be able to enter gain values and a desired heading before operation, and should then be able to see the current battery voltage state during operation. This code remained largely constant throughout the project, with some additional input being necessary for lab 6 to implement the additional gains. Initialization Ports, PCA, ADC, XBR0 Port initialization function sets the output pins for CEX0, CEX1, CEX2, and CEX3 in push pull mode. These are on port 1, pins 0-3. The other pins on port one are set. Port 1 pin 5 is set for analog input. Then port 0 is set. XBR0 initialization is just setting XBR0 to 0x27. This ensures that CEX0, CEX1, CEX2 and CEX3 are all working correctly. PCA initialization sets the PCA interrupt to be enabled with SYSCLK/12. It also sets CCM0, CCM1, CCM2 and CCM3 in 16 bit compare mode, as well as enabling the PCA counter. The ADC initialization function sets Vref to the internal reference voltage, and also enables the AD converter. Use of the PCA The Programmable Counter Array, or PCA, was used in each lab to generate the pulse streams to control the direction and speed of the vehicles and to serve as a timer to control when functions are called. In order to streamline the transition between labs, certain standards were created. In the initialization function for the PCA, the system uses SYSCLK/12 as the counter for the interrupt service routine. Each capture compare module is initialized to a standard 16 bit compare mode, as seen in the code below:
PCA0MD = 0x81; PCA0CPM0 = 0xC2; PCA0CN = 0x40; //PCA interrupt enable with SYSCLK/12 //CCM0 in 16-bit compare mode //Enable PCA counter

This PCA initialization function remains largely the same for each of the labs. When generating a pulse width signal to control the servos and motors, the code sets the corresponding CCM. In each of the labs, CCM 0 was the CCM that controlled the heading of the vehicle, and CCM 2 controlled the speed. During labs 3, 4 and 5, CCMs 1 and 3 were not used. In lab 6, CCM 1 controlled the angle of the thrust fans, and CCM 3 controlled the speed of the right thrust fan while CCM 2 controlled the speed of the left thrust fan. The PCA ISR also served as a timer to allow the software to avoid overloading different components such as the LCD display. This was done through a 20 ms pause function. Pulse Streams The method used to control the actions of the servo and the motors is known as Pulse Width Modulation, or PWM. This refers to the fact that the PCA will send either a low or a high signal depending on the relation between the current value and that stored in the CCM. By changing the values stored in the CCM, the amount of time that the signal is high, or the Pulse Width, can be changed. The PCA outputs a high signal if the count is higher than the value stored in the CCM. The PCA outputs as many unique signals as there are unique CCMs. Each pulse width is bounded based on the physical requirements for the device in question. In lab 6 all devices had a minimum pulse width of 2000 and a maximum pulse width of 3500. In order to avoid damage to the devices all pulse widths must be within this range. In order to set a pulse width, the CCM must be loaded with the desired value. If the pulse width is known, then the CCM must be set to the maximum value minus the desired value. This ensures that the PCA will only return a high signal over the desired duration. As each CCM has both a high and a low byte, each of these must be set in order to set the CCM properly. A sample PW setting is shown below:
PCA0CPL2 = 0xFFFF - PW; //sets low byte, PW = 2750

PCA0CPH2 = (0xFFFF - PW) >> 8; //sets high byte, PW = 2750

In the example above, the 2nd CCM is being set to a value of 62785 (65535 2750). As the counter overflows at 65536, this ensures that the PCA will send a high signal for 2750 counts, creating a pulse width of 2750. In labs 3, 4, 5 and 6, the pulse width was a function of how far from the desired heading the car was. This is known as proportional damping, and was applicable due to the high amounts of natural damping due to friction. The formula to find the desired pulse width to control the car was therefore;
PW = PW_NEUTRAL + Kp * (DESIRED_VALUE ACTUAL_VALUE)

where Kp is a calculated gain value and the data for the values is either heading or acceleration, and range. For lab 6, the lack of significant amounts of friction required the addition of a derivative gain term in order to mimic the damping effects of friction. Thus the formula to find pulse width became:
PW = PW_NEUTRAL + Kp*(DESIRED_VALUEACTUAL_VALUE) + Kd*(ERROR-PREVIOUS_ERROR)

By adding the final term, the PW can be controlled both in terms of the current heading and the current error value. This makes the gondola much less likely to swing in circles, and much quicker to settle on the desired heading. I2C Read and Write The two options for I2C data transfer are reading and writing. These functions allow the master to receive and send data respectively, and have functions to carry out these commands hard coded into the i2c.h file. The exact functions can be found on pages 149 and 150 of the lab manual. In these labs the write command is useless so we will focus on the read command. In this action, the master sends a data array to the slave location. This array is written to, and the data in it is analyzed. This process repeats until there is no more data to be read. An example in which the compass is read is shown below:
i2c_read_data(0xC0, 0x02, Data_heading, 2); heading =(((unsigned int)Data_heading[0] << 8) | Data_heading[1]);

This code segment retrieves the data from the compass and saves it to the heading variable. The address of the compass is known to be 0xC0, and the desired data is known to start at 0x02. The first segment of the data array is the high byte, and the second is the low byte. Use of I2C: In labs 3, 4, 5, and 6 there is a need to get information from peripheral devices like the electronic compass and use it in the microcontroller. Therefore, there needs to be a communications protocol so that this data can be transferred. There are two types of communications processes, serial communications and parallel communications. Serial communications has the advantage of only requiring two wires and ports so that is the type of communications protocol that was used. These two wires are known as the SDA and SCL wires, or data and clock. The data wire is used to send one byte of information at a time, while the clock wire is used to synchronize the two devices. In this configuration the C8501 is the master and the data devices are slaves which communicate with the master. When the SMBUS is initialized, the SCL frequency is set to 100 kHz and both lines are set to open drain, as they can serve as both an input and an output line. Reading Analog Input, A/D Results To read the analog input, we make sure that ADC is initialized. After that, we can call the read AD input function. This function takes a reading off of port 1 pin 5 and waits until it is done with the conversion. After it is finished, it returns the value that was received. After this value is returned, we run a simple calculation which gives us the current battery voltage based on what the input value of the ADC was as well as the circuit, and resistance that was used. Keypad Input & LCD output 10

Reading a keypad input is very easy. All that needs to be done, is call the read key function. This gets the input on the keypad. If nothing was pressed, it repeats after a short pause. After a key is pressed, the value is returned. If the integer value is wanted, this value should be reduced by 48. To print out on the LCD output, simply call the lcd_print() function. If variables are being output, they need to be in character format.= Control Loops In order to control the gondola in lab 6, PD error correction was used. Once the initial error was calculated, both a derivative and proportional error were added. This means the gondola should move quickly to the intended direction, not overshoot very far, and if it does overshoot, it should come back quickly and not oscillate very much. However, if any of the gain values are too large or too small, the gondola may move too quickly and spin in circles, oscillate very quickly, and/or never reach its intended heading. Timing All of the timing for this system was done using PCA interrupts. Three counters were used; r_count, countsrange, and countsheading. Each of these was incremented about every 20 miliseconds. This was set by setting the PCA0L and PCA0H to 28672. After r_count got larger than 4 (80 ms) it was reset to zero and a flag to get the new range was set.

11

Results & Conclusions

Analysis of Results Desired heading for all trials is XX. Data has been collect for the error vs. time of the heading control algorithm. We are using a proportional plus derivative algorithm which has two gain term Kp and Kd. These will be adjusted and the changes in system response will be observed. The gain values have been adjusted according to the table below: Case 1 2 3 4 5 6 7 8 Kp 0.1 5 0.1 0.5 3 3 12 12 Table 1 After inputting the suggested proportional and derivative gains, a graph was generated that included every case. The cases can be broken down into three main sub-groups based on the pattern the respective line followed in the graph. With a high derivative gain and relatively low proportional gain, the line approached the optimal value slowly, and never exceeded it. This pattern equates to the gondola slowly rotating toward its desired heading, but never surpassing it. Another case was a dampening sinusoidal wave, which bounced over and under the optimal value until the gondola landed appropriately at its desired heading. This case corresponds to a relatively high proportional gain, with a smaller derivative gain. The final case was when the gondola simply ended up spinning in circles and never remaining at the optimal value. In this instance, the proportional gain was very high compared to the derivative gain, so the gondola would pick up too much rotational speed upon approaching its desired heading. In the end, a couple more cases were tried to find the optimal setting for the different gains. The best set of gains determined was setting 4 for the proportional gain and 30 for the derivative gain. Using these values, the gondola quickly approached the desired heading and slowed down just in time to rest at the desired value. These plots are located in Figure XX on the following page. 12 Kd 0 0 10 70 70 180 70 180

4000

Current Heading versus Time

3500

3000

optimal 2500 Current Heading (degrees * 10) kp=0.1, kd=0 kp=5, kd=0 kp=0.1, kd=10 kp=0.5, kd=70 2000 kp=3, kd=70 kp=3, kp=180 kp=12, kd=70 kp=12, kd=180 1500 kp=2, kd=35 kp=4, kd=20 kp=4,kd=30 kp=30, kd=70 1000

500

0 1 9 17 25 33 41 49 57 65 73 81 89 97 105 113 121 129 137 145 153 161 169 177 185 193 201 209 Time (each data point corresponds to 20 milliseconds)

13

Description of gondola performance The Cars feedback system used the accelerometer data to the errors from the neutral position drive the Car up a wooden ramp to this position. The x-coordinate steers the Car while driving it up the ramp until the yaw is neutral. The y-coordinate drive the Car up the ramp until the tilt has reached the neutral position. The gains of the proportional control loops can be adjusted by user input via the numeric keypad. This control loop was quite predictable and we encountered no major problems in getting our Car to behave as desired. The Gondolas first control algorithm is the thrust loop which will cause the blimp to hover at a predetermined altitude. Two thrust fans set vertically by the thrust angle servo propel the Gondola. On the Blimp, the Gondola is expected to achieve the desired altitude with a quick response time and minimized system over shoot. The second control loop will achieve the Gondolas desired heading. This loop drives the tail fan to achieve a desired heading similarly to the previous loop with quick response and minimized overshoot. The algorithm also required the shortest path be taken to acquire the heading. The desired heading and control algorithm gain terms are entered by the used via the numeric keypad. The Gondola attempts to satisfy both conditions as fast as possible within the limits set by the control loop. After trouble shooting some problems we were able to get the Gondola to function as expected. These problems will be discussed in detail below. For completeness the control algorithms will be explained. In our applications the proportion gain is linearly modulates speed or position of a motor between neutral and extreme states given the error from the desired position. A small proportional gain will result in slow response time however a large gain value will result in unwanted oscillations as the system overshoots its target infinitely. This is where the derivative gain term is useful in damping the response of the proportional gain as the loop approaches its target. The change in error over time is calculated and multiplied by the differential gain helping decreased system overshoot for a high proportional gain term. What was Learned Over this semester we have learned a tremendous amount about building and programming embedded control systems. There is much more to learn however the consensus is that we have enough knowledge to explore advanced systems independently. I personally feel like this class has expanded my engineering breath and that I will be able to utilize a lot of this knowledge in my professional career. We are able to safely create hardware configurations from schematics, initialize and calibrate the microcontroller, write C code to communicate with all of our systems with functions to execute desired control and finally quantify the performance of the system. Problems Encountered & Solution We had a problem getting our PCA signals to the right capture and compare modules on the Gondola. We are not sure if it was a hardware issue but the motors would sometimes behave erratically requiring the Gondola to be shutdown with the program recompiled and reloaded. There were also issues with our heading control loop which would infinitely overshoot the desired heading. We tried changing the battery multiple times but this did not resolve the 14

problem. We eventually resolved these issues by making adjustments to our initialization values and control loop but the specific solution is unknown.

References
P. Schoch, A. Gutin, S. Lee, C. Sankar, LITEC Lab Manual Version 14.2, 2012

Participation We had great participation from our group and we all learned valuable information from this class. Sam Ellis was responsible for the hardware description, data collection and analysis, and schematics. Eric Fish was responsible for the Lab code and software description, data analysis. Michael Fredericks was responsible for the software description and flow chart. Sonike Hendricks was responsible for the abstract, purpose and objectives, overview of micro controller, data and analysis.

Sam Ellis

Sonike Hendricks

________________________________

________________________________

Eric Fish

Michael Fredericks

________________________________

________________________________

15

Source Code
Car Code Lab 5
#include #include #include #include <c8051_SDCC.h>// include files. <stdio.h> <stdlib.h> <i2c.h> //2030 = 1.1 ms pulsewidth is min // ( ( 1.1/20 ) * 36863 per pulse ) //3500 = 1.9 ms pulsewidth is max // ( (1.9/20) * 36863 per pulse ) //2760 = 1.5 ms pulsewidth is off

#define MOTOR_PW_MIN 2030 #define MOTOR_PW_MAX 3500 #define MOTOR_PW_NEUT 2760

//----------------------------------------------------------------------------// 8051 Initialization Functions //----------------------------------------------------------------------------void Port_Init(void); void PCA_Init (void); void SMB_Init(void); void XBR0_Init(void); void Interrupt_Init(void); void ADC_Init(void); void Drive_Motor(void); //controls the speed of vehicle void Start_Ping(); //start ping function unsigned int ReadRanger(); //read ranger function unsigned int getHeading(void); //read compass void steer(void); //control heading of vehicle char read_key(void); //get LCD input void pause(void); //wait 20 ms unsigned char read_AD_input(void); //read input from the voltage void get_tilt(void); //read the accelerometer //----------------------------------------------------------------------------// Global Variables //----------------------------------------------------------------------------unsigned int MOTOR_PW = 0; //initial motor power is 0 unsigned int countsrange = 0; //used for general timing unsigned int range = 0; //int for storing data from ranger unsigned int PCA_START_1 = 28672; signed int errorrange; __sbit __at 0xB3 LED; __sbit __at 0xB6 SS; //pca start for 20 ms pulse //error in the current range //LED to see on/off //Slide Switch for on/off

signed int avg_gx = 0; //x-axis accelerometer signed int avg_gy = 0; //y-axis accelerometer signed int temp_gy = 0; unsigned char countsheading = 0; unsigned char Data_heading[4]; // Data_heading is an array with a length of 2 unsigned int HEADING_PW_MIN = 2143; //limits unsigned int HEADING_PW_MAX = 3173; unsigned int PW = 0; float k = .1; //gain char keypad, keypad1, keypad2, keypad3; //used for inputs unsigned char output; float voltage; int i = 0;

16

//----------------------------------------------------------------------------// Main Function //----------------------------------------------------------------------------void main(void) { // initialize all systems Sys_Init(); putchar(' '); //the quotes in this line may not format correctly Port_Init(); XBR0_Init(); PCA_Init(); Interrupt_Init(); SMB_Init(); ADC_Init(); Accel_Init(); LED = 1; //print beginning message printf("\r\nEmbedded Control Drive Motor Control\r\n"); //Enter gain, pauses used to avoid breaking keypad lcd_clear(); pause(); lcd_print("Please enter a desired gain: "); keypad1 = read_key(); keypad2 = read_key(); keypad3 = read_key(); k k k k = 0; += (keypad1-48)/10.0; += (keypad2-48)/100.0; += (keypad3-48)/1000.0; //calls read key function to read input

//sets gain value

printf_fast_f("k: %4.2f\r\n", k); pause(); lcd_clear(); pause(); //print gain values to LCD and SDC lcd_print("%c%c%c", keypad1,keypad2,keypad3); printf("gain is %c%c%c \r\n",keypad1,keypad2,keypad3); pause(); lcd_clear(); pause(); //set initial value MOTOR_PW = MOTOR_PW_NEUT; //set ititial heading values countsheading = 0; PW = 2725; PCA0CPL0 = 0xFFFF - PW; PCA0CPH0 = (0xFFFF - PW) >> 8; //turn motor off

//initially set the wheels to go straight

17

//add code to set the servo motor in neutral for one second while(countsrange < 50 ) { MOTOR_PW = MOTOR_PW_NEUT; } printf("Running\r\n"); while(1) { if(!SS) // if the slide switch is in the on position { LED = 0; //read Ranger if(!(countsrange % 4)) { printf("i am here \r\n"); steer(); //call steering function to get a heading and set the wheels properly printf("The Range is: %u\r\n", range); output = read_AD_input();//calculates the voltage voltage = output/255.0; voltage *=30; //gain to adjust voltage value if(countsrange > 20) { //print voltage gain and heading to LCD keypad = (int)(voltage/10)+48; //setup for printing to LCD keypad1 = ((int)voltage%10)+48; keypad2 = ((int)(voltage*10)%10)+48; keypad3 = ((int)(voltage*100)%10)+48; lcd_clear(); lcd_print("Drive PW:%u", MOTOR_PW); lcd_print("\nSteer PW:%u", PW); lcd_print("\nVoltage: %c%c", keypad, keypad1); lcd_print(".%c%c", keypad2, keypad3 ); lcd_print("\naccelX:%d", avg_gx); lcd_print(" accelY:%d", avg_gy); countsrange = 0; printf_fast_f("Voltage: %4.2f", voltage); } } } else if (SS) //if the slide switch is in the off position { LED = 1; MOTOR_PW = MOTOR_PW_NEUT; PCA0CPL2 = 0xFFFF - MOTOR_PW; PCA0CPH2 = (0xFFFF - MOTOR_PW) >> 8; while(countsrange < 100); printf("turn on the Slide Switch to run\r\n"); output = read_AD_input();//same as above for voltage voltage = output/255.0; voltage *=30; keypad = (int)(voltage/10)+48;

18

keypad1 = ((int)voltage%10)+48; keypad2 = ((int)(voltage*10)%10)+48; keypad3 = ((int)(voltage*100)%10)+48; lcd_clear(); lcd_print("Voltage: %c%c", keypad, keypad1); lcd_print(".%c%c", keypad2, keypad3 ); countsrange = 0; printf_fast_f("Voltage: %4.2f", voltage); } } } //----------------------------------------------------------------------------// Drive_Motor //----------------------------------------------------------------------------// // Vary the pulsewidth based on the user input to change the speed // of the drive motor. // void Drive_Motor() { printf("drivemotor\r\n"); if (avg_gx < 60 || avg_gx > 0xFE00) //if the car is tilting in the x direction { avg_gx = 60 - avg_gx; MOTOR_PW = MOTOR_PW_NEUT + 6*(avg_gx);//function to control drive printf("MOTOR_PW = %d\r\n", MOTOR_PW); if (MOTOR_PW > MOTOR_PW_MAX) //limits range of PW values MOTOR_PW = MOTOR_PW_MAX; PCA0CPL2 = 0xFFFF - MOTOR_PW; //set CCM to the PW value PCA0CPH2 = (0xFFFF - MOTOR_PW) >> 8; } else if (avg_gy > 50 || avg_gy < 20) //if the car is tilting in the y direction { MOTOR_PW = MOTOR_PW_NEUT + (450); //function to control drive printf("MOTOR_PW = %d\r\n", MOTOR_PW); PCA0CPL2 = 0xFFFF - MOTOR_PW; PCA0CPH2 = (0xFFFF - MOTOR_PW) >> 8; } else //if object is in desired range { MOTOR_PW = MOTOR_PW_NEUT; //motor is not running printf("MOTOR_PW = %d\r\n", MOTOR_PW); PCA0CPL2 = 0xFFFF - MOTOR_PW; PCA0CPH2 = (0xFFFF - MOTOR_PW) >> 8; } } //----------------------------------------------------------------------------// Port_Init //----------------------------------------------------------------------------// // Set up ports for input and output //

19

void Port_Init() { P1MDOUT |= 0x05; ;//set output pin for CEX0, CEX2 in push-pull mode P1MDIN &= ~0x80; P1MDOUT &= ~0x80; P1 |= 0x80; P0MDOUT |= 0x03; } void ADC_Init(void) { REF0CN = 0x03; ADC1CN = 0x80; }

//set Vref to internal ref voltage //enable AD converter

unsigned char read_AD_input(void) { AMX1SL = 7; ADC1CN = ADC1CN & ~0x20; ADC1CN = ADC1CN | 0x10; while ((ADC1CN & 0x20) == 0x00); return ADC1; } //----------------------------------------------------------------------------// Interrupt_Init //----------------------------------------------------------------------------// // Set up ports for input and output // void Interrupt_Init(void) { // IE set via glogbal EA assignment EA = 1; // enable global interrupts // EIE1 EIE1 |= 0x08; //enable PCA interrupt } //----------------------------------------------------------------------------// XBR0_Init //----------------------------------------------------------------------------// // Set up the crossbar // void XBR0_Init(void) { XBR0 = 0x27; //configure crossbar with UART, SPI, SMBus, and CEX channels as // in worksheet } //----------------------------------------------------------------------------// PCA_Init //----------------------------------------------------------------------------// // Set up Programmable Counter Array // void PCA_Init(void) { // reference to the sample code in Example 4.5 // Pulse Width Modulation implemented us-ing the PCA // while the conversion is not completed

20

// (Programmable Counter Array, p. 50 in Lab Manual. // Use a 16 bit counter with SYSCLK/12. PCA0MD = 0x81; //PCA interrupt enable with SYSCLK/12 PCA0CPM0 = 0xC2; //CCM0 in 16-bit compare mode PCA0CPM2 = 0xC2; //CCM2 in 16-bit compare mode PCA0CN = 0x40; //Enable PCA counter } //----------------------------------------------------------------------------// SMB_Init //----------------------------------------------------------------------------// // Set up SMBUS // void SMB_Init(void) { SMB0CR = 0x93; ENSMB = 1; }

//Set SCL to 100kHz //Enble SMBUS (Bit 6 of SMB0CN)

//----------------------------------------------------------------------------// PCA_ISR //----------------------------------------------------------------------------// // Interrupt Service Routine for Programmable Counter Array Overflow Interrupt // void PCA_ISR ( void ) __interrupt 9 { // reference to the sample code in Example 4.5 // Pulse Width Modulation implemented using the PCA // (Programmable Counter Array) if (CF) { CF = 0; //clear overflow flag countsrange++; countsheading ++; PCA0L = PCA_START_1; PCA0H = PCA_START_1; } PCA0CN &= 0xC0; } unsigned int getHeading(void){ //get the acceleration data i = 0; avg_gx = 0; avg_gy = 0; while(i < 8){ //need several headings to average pause(); get_tilt(); i++; } avg_gx = avg_gx/8; avg_gy = avg_gy/8; printf("%u, %u\r\n", avg_gx, avg_gy); //print the accelerations //handle other PCA interrupt sources //increment wait counter //reading counter //low byte of start count, 20ms //high byte of start count, 20, ms

21

if(avg_gy > 50) {

//set the PW depending on acceleration

PW = 2725 - 2*(avg_gy - 50); } else if (avg_gy < 30 || avg_gy > 0xFE00){ temp_gy = 30-avg_gy; printf("%u\r\n\r\n", temp_gy); PW = 2725 + (3*temp_gy); printf("%u, %u, %u\r\n", PW, 2725, temp_gy); } else PW = 2725; //neutral PW Drive_Motor(); calculated return PW; } //drive the motor after the acceleration data has been

void steer(void){ if(SS){ //if the slide switch is in the off position PW = 2725; //set the wheels to center PCA0CPL0 = 0xFFFF - PW; PCA0CPH0 = (0xFFFF - PW) >> 8; printf("returning? \r\n"); return; //then do nothing } if(countsheading >= 3){ //otherwise make sure that there is a new compass reading countsheading = 0; printf("getheading\r\n"); PW = getHeading(); //get a new heading printf("got heading %d\r\n", PW); } printf("afterif \r\n"); if(PW < HEADING_PW_MIN) PW = HEADING_PW_MIN; //if it is less than the min limit, set it to the min limit if(PW > HEADING_PW_MAX) PW = HEADING_PW_MAX; //same with max limit PCA0CPL0 = 0xFFFF - PW; //output the desired PW PCA0CPH0 = (0xFFFF - PW) >> 8; } char read_key(void) { keypad = read_keypad(); pause(); // This pauses for 1 PCA0 counter clock cycle (20ms) //If the keypad is read too frequently, it will // lock up and stop responding. Must power down to reset. while (keypad == -1) { keypad = read_keypad(); pause(); } if (keypad != -1) // keypad = -1 if no key is pressed { // Note: fast read results in multiple lines on terminal // A longer delay will reduce multiple keypad reads but a // better approach is to wait for a -1 between keystrokes. pause(); while (read_keypad() != -1)

22

{ pause(); } } return keypad; } void pause(void) { int n_counts = countsheading; while (countsheading - n_counts < 1);// 1 count -> (65536-PCA_START) x 12/22118400 = 20ms } void get_tilt(void){ //i2c read function that gets the acceleration data from the accelerometer i2c_read_data(0x30, 0x28|0x80, Data_heading, 4); avg_gx += ((Data_heading[1] << 8 | Data_heading[0]) >> 4); avg_gy += ((Data_heading[3] << 8 | Data_heading[2]) >> 4);

23

Gondola Code Lab 6

#include #include #include #include <c8051_SDCC.h>// include files. <stdio.h> <stdlib.h> <i2c.h>

//----------------------------------------------------------------------------// 8051 Initialization Functions //----------------------------------------------------------------------------void Port_Init(void); //port initialization function void PCA_Init (void); //initialize PCA void SMB_Init(void); //initialize system bus void XBR0_Init(void); //set up the crossbar void Interrupt_Init(void); //set up interrupts void Input(void); //get input for gains and heading void Drive_Motor(void); //function for controlling 2 outside fans void Start_Ping(); //start ping function unsigned int ReadRanger(); //read ranger function unsigned int getHeading(void); //heading control function void steer(void); //function for controlling back fan char read_key(void); //reads inputs on the keypad void pause(void); //wait 20 ms void ADC_Init(void); //initializes voltage read unsigned char read_AD_input(void); int PD_Calc(int desired, int actual, int previous); //calculate fan speed unsigned char inputGains(void); void resetKeys(void); //reset char variables for sequential input char read_input(void); //get input from LCD keypad //----------------------------------------------------------------------------// Global Variables //----------------------------------------------------------------------------unsigned int MOTOR_PW = 0; //initial motor power is 0 unsigned int countsrange = 0; //used for general timing unsigned int r_count = 0; //ranger counter unsigned char new_range = 0; //flag for pinging for a new range unsigned int range = 0; //int for storing data from ranger //unsigned int PCA_START_1 = 28672; signed int errorrange; //pca start for 20 ms pulse

unsigned char countsheading = 0; unsigned char Data_heading[2]; // Data_heading is an array with a length of 2 unsigned int heading; // the heading returned in degrees between 0 and 3599 unsigned int PW = 0; unsigned int desiredHeading = 0; //set desired heading here (in this case E) signed int errorheading; unsigned char kp; //gain for steering fan unsigned char kd; //derivative gain for steering fan unsigned char kpd; //proportional gain for height fan unsigned char kdd; //derivative gain for height fan char keypad, keypad1, keypad2, keypad3; float voltage; //----------------------------------------------------------------------------// Main Function //----------------------------------------------------------------------------void main(void) { // initialize board

24

Sys_Init(); putchar(' '); //the quotes in this line may not format correctly Port_Init(); XBR0_Init(); PCA_Init(); Interrupt_Init(); SMB_Init(); ADC_Init(); PCA0CPL1 = 0xFFFF - 2750; PCA0CPH1 = (0xFFFF - 2750) >> 8; printf("rudders set\r\n"); //print beginning message printf("\r\nEmbedded Control Drive Motor Control\r\n"); Input(); //get inputs from keyboard //add code to set the servo motor in neutral for one second while(countsrange < 50 ) { MOTOR_PW = 2750; } printf("Running\r\n"); printf("Error Heading will be displayed after inputs\r\n"); while(1) { //read Ranger if (new_range == 1) //print range { steer(); //call steering function to get a heading and set the fan properly new_range = 0; range = ReadRanger(); Drive_Motor(); Start_Ping(); //printf("The Range is: %u\r\n", range); keypad = read_AD_input(); voltage = keypad/255.0; voltage *=27; if(countsrange > 20) { //set the keypad for voltage display keypad = (int)(voltage/10)+48; keypad1 = ((int)voltage%10)+48; keypad2 = ((int)(voltage*10)%10)+48; keypad3 = ((int)(voltage*100)%10)+48; countsrange = 0; } } } } //set thrust fan angle //set thrust fan angle

25

//Input void Input() { //Enter gains lcd_clear(); pause(); lcd_print("Please enter a desired gain kp kd kpd kdd: "); printf("Please enter a desired gain kp kd kpd kdd: "); desiredHeading = 0; kp = inputGains(); //reset to avoid overloading keypad pause(); resetKeys(); pause(); kd = inputGains(); //reset to avoid overloading keypad pause(); resetKeys(); pause(); kpd = inputGains(); //reset to avoid overloading keypad pause(); resetKeys(); pause(); kdd = inputGains(); //reset to avoid overloading keypad pause(); resetKeys(); pause(); //enter desired heading, pauses to avoid overload pause(); lcd_clear(); pause(); lcd_print("Please enter a desired heading in degrees: "); pause(); pause(); pause(); pause(); keypad1 = getchar(); pause(); pause(); keypad2 = getchar(); pause(); pause(); keypad3 = getchar(); pause(); pause(); pause(); printf("direction is %c%c%c \r\n",keypad1,keypad2,keypad3); //calculate desired heading

26

desiredHeading += (keypad1-48)*100; desiredHeading += (keypad2-48)*10; desiredHeading += (keypad3-48)*1; while (desiredHeading >= 3600) { desiredHeading -= 3600; } //set heading to useful value if user messes up

pause(); lcd_clear(); pause(); pause(); //print heading printf("\r\n\n %d %d %d %d \r\nHeading:%d", kp, kd, kpd, kdd, desiredHeading); printf("Now printing error in Heading below\r\n"); lcd_print("Your heading is: %c%c%c ", keypad1, keypad2,keypad3); //set initial value MOTOR_PW = 2750; //neutral //set ititial heading values countsheading = 0; PW = 2750; PCA0CPL0 = 0xFFFF - PW; PCA0CPH0 = (0xFFFF - PW) >> 8; PCA0CPL2 = 0xFFFF - 2750; PCA0CPH2 = (0xFFFF - 2750) >> 8; PCA0CPL3 = 0xFFFF - 2750; PCA0CPH3 = (0xFFFF - 2750) >> 8; } //----------------------------------------------------------------------------// Drive_Motor //----------------------------------------------------------------------------// // Vary the pulsewidth based on the user input to change the speed // of the drive motor. // void Drive_Motor() { if (range >= 55) { errorrange = 50 - range; //errorrange is the distance from the desired distance if (errorrange < -40) //if the object is too far away { errorrange = -40; //cap the value of errorrange } //if (MOTOR_PW > MOTOR_PW_MIN) MOTOR_PW = PD_Calc(50, range, errorrange); //function to control drive if (MOTOR_PW < 2500 || MOTOR_PW < -1) { MOTOR_PW = 2500; } //set thrust fans to calculated PW PCA0CPL2 = 0xFFFF - MOTOR_PW; PCA0CPH2 = (0xFFFF - MOTOR_PW) >> 8; PCA0CPL3 = PCA0CPL2; PCA0CPH3 = PCA0CPH2; }

//set heading fan to neutral //set thrust fans to neutral //set thrust fans to neutral

27

else if (range <= 45) { errorrange = 40 - range; //errorrange is the distance from the desired distance if (errorrange > 30) //if the object is too close { errorrange = 30; //cap the value of errorrange } //if (MOTOR_PW < MOTOR_PW_MIN) MOTOR_PW = PD_Calc(50, range, errorrange); //function to control drive if (MOTOR_PW > 3000) { MOTOR_PW = 3000; } PCA0CPL2 = 0xFFFF - MOTOR_PW; PCA0CPH2 = (0xFFFF - MOTOR_PW) >> 8; PCA0CPL3 = PCA0CPL2; PCA0CPH3 = PCA0CPH2; } else { MOTOR_PW PCA0CPL2 PCA0CPH2 PCA0CPL3 PCA0CPH3 } } //----------------------------------------------------------------------------// ReadRanger //----------------------------------------------------------------------------// // Read the Ranging device every 80 ms // return a distance in cm (stored in an int) // unsigned int ReadRanger() //read ranger function { unsigned char Data[2]; unsigned int range = 0; unsigned char addrrange = 0xE0; // the addrrangeess of the ranger is 0xE0 //read data i2c_read_data(addrrange, 0x02 , Data, 2); // read two bytes, starting at reg 2 range = (((unsigned int)Data[0] << 8) | Data[1]); //calculate range return range; } //----------------------------------------------------------------------------// Start_Ping //----------------------------------------------------------------------------// // send out a ping for the ranger to read // void Start_Ping() = = = = = //if object is in desired range 2750; //motor is not running 0xFFFF - MOTOR_PW; (0xFFFF - MOTOR_PW) >> 8; 0xFFFF - MOTOR_PW; (0xFFFF - MOTOR_PW) >> 8;

28

{ unsigned char Data[1]; unsigned int range = 0; unsigned char addrrange = 0xE0; // the addrrangeess of the ranger is 0xE0 //Send ping (before reading?) Data[0] = 0x51 ; // write 0x51 to reg 0 of the ranger - ranging mode in centimeters: i2c_write_data(addrrange, 0x00, Data, 1) ; // write one byte of data to reg 0 at addrrange } //----------------------------------------------------------------------------// Port_Init //----------------------------------------------------------------------------// // Set up ports for input and output // void Port_Init() { P1MDOUT |= 0x0F; ;//set output pin for CEX0, CEX1, CEX2, CEX3 in push-pull mode P1MDIN &= ~0x20; P1MDOUT &= ~0x80; P1 |= 0x20; P0MDOUT |= 0x03; } void ADC_Init(void) { REF0CN = 0x03; ADC1CN = 0x80; }

//set Vref to internal ref voltage //enable AD converter

unsigned char read_AD_input(void) { AMX1SL = 5; ADC1CN = ADC1CN & ~0x20; ADC1CN = ADC1CN | 0x10; while ((ADC1CN & 0x20) == 0x00); //while conversion is not done, wait return ADC1; } //----------------------------------------------------------------------------// Interrupt_Init //----------------------------------------------------------------------------// // Set up ports for input and output // void Interrupt_Init(void) { // IE set via glogbal EA assignment EA = 1; // enable global interrupts // EIE1 EIE1 |= 0x08; //enable PCA interrupt } //----------------------------------------------------------------------------// XBR0_Init //----------------------------------------------------------------------------//

29

// Set up the crossbar // void XBR0_Init(void) { XBR0 = 0x27; //configure crossbar with UART, SPI, SMBus, and CEX channels as // in worksheet } //----------------------------------------------------------------------------// PCA_Init //----------------------------------------------------------------------------// // Set up Programmable Counter Array // void PCA_Init(void) { // reference to the sample code in Example 4.5 // Pulse Width Modulation implemented using the PCA // (Programmable Counter Array, p. 50 in Lab Manual. // Use a 16 bit counter with SYSCLK/12. PCA0MD = 0x81; //PCA interrupt enable with SYSCLK/12 PCA0CPM0 = 0xC2; //CCM0 in 16-bit compare mode PCA0CPM1 = 0xC2; //CCM1 in 16-bit compare mode PCA0CPM2 = 0xC2; //CCM2 in 16-bit compare mode PCA0CPM3 = 0xC2; //CCM3 in 16-bit compare mode PCA0CN = 0x40; //Enable PCA counter } //----------------------------------------------------------------------------// SMB_Init //----------------------------------------------------------------------------// // Set up SMBUS // void SMB_Init(void) { SMB0CR = 0x93; ENSMB = 1; }

//Set SCL to 100kHz //Enble SMBUS (Bit 6 of SMB0CN)

//----------------------------------------------------------------------------// PCA_ISR //----------------------------------------------------------------------------// // Interrupt Service Routine for Programmable Counter Array Overflow Interrupt // void PCA_ISR ( void ) __interrupt 9 { // reference to the sample code in Example 4.5 // Pulse Width Modulation implemented using the PCA // (Programmable Counter Array) if (CF) { CF = 0; //clear overflow flag r_count++; countsrange++; countsheading ++; PCA0L = 28672; //increment pulse counter //increment wait counter //reading counter //low byte of start count, 20ms

30

PCA0H = 28672; if (r_count >= 4) { new_range = 1; r_count = 0; } } PCA0CN &= 0xC0; } unsigned int getHeading(void){

//high byte of start count, 20, ms // 4 overflows is about 80 ms //flag to get a new range

//handle other PCA interrupt sources

Data_heading[0] = 0; Data_heading[1] = 0; i2c_read_data(0xC0, 0x02, Data_heading, 2); // read two byte, starting at reg 2 //printf("Data[0] = %U\r\n", Data[0]); //printf("Data[1] = %U\r\n", Data[1]); heading =(((unsigned int)Data_heading[0] << 8) | Data_heading[1]); //combine the two values //heading has units of 1/10 of a degree return heading; // the heading returned in degrees between 0 and 3599 }

void steer(void){ if(countsheading >= 3){ //otherwise make sure that there is a new compass reading countsheading = 0; heading = getHeading(); //get a new heading printf("%d\r\n", heading); } else return; //if there is not a new heading do nothing //printf("got heading %d\r\n", heading); PW = PD_Calc(desiredHeading, heading, errorheading); //calculate a new PW value //printf("PW errorheading %d\r\n", PW); //print it out if(PW < 2000) PW = 2000; //if it is less than the min limit, set it to the min limit if(PW > 3500) PW = 3500; //printf("%u\r\n", PW); PCA0CPL0 = 0xFFFF - PW; PCA0CPH0 = (0xFFFF - PW) >> 8; MOTOR_PW = PW; } char read_key(void) { char key = read_keypad(); pause(); // This pauses for 1 PCA0 counter clock cycle (20ms) // If the keypad is read too frequently (no delay), it will // lock up and stop responding. Must power down to reset. while (key == -1) { key = read_keypad(); pause(); } //same with max limit //output the desired PW

31

if (key != -1) // keypad = -1 if no key is pressed { // Note: fast read results in multiple lines on terminal // A longer delay will reduce multiple keypad reads but a // better approach is to wait for a -1 between keystrokes. pause(); while (read_keypad() != -1) { pause(); } //lcd_clear(); //lcd_print("Your key was:\n %c, = Hex %X", keypad, keypad); //printf("\n\rYour key was: %c, = Hex %X", keypad, keypad); // printf("\r\nDec: %u", keypad); } return key; } int PD_Calc(int desired, int actual, int previous) { int temp_motorpw; if(previous == errorheading){ //if the readings are the same, recalculate errorheading errorheading = desired - actual; if(errorheading > 1800) errorheading -= 3600; if(errorheading < -1800) errorheading += 3600; //calculate motor pw using proportional and derivative control temp_motorpw = (long)2750+(long)kp*(long)errorheading+(long)kd*(long)(errorheading-previous); } else{ errorrange = desired - actual; //calculate motor pw using proportional and derivative control temp_motorpw = (long)(2750)+(long)kpd*(long)errorrange+(long)kdd*(long)(errorrange-previous); } return temp_motorpw; } unsigned char inputGains(void){ unsigned char k = 0; unsigned char key1, key2, key3; //input gain from computer pause(); key1 = getchar(); pause(); key2 = getchar(); pause(); pause(); key3 = getchar(); pause(); k += (key3-48); k += (key2-48)* 10; k += (key1-48)* 100; //print gain values pause(); lcd_print(" %c%c%c", (key1),(key2),(key3));

32

pause(); printf("4"); printf("k= %d",k); return k; } void resetKeys(void){ //set all reset all characters to allow new information to be used keypad = 0; keypad1 = 0; keypad2 = 0; keypad3 = 0; } void pause(void) { //wait for 1 ccount int n_counts = countsheading; while (countsheading - n_counts < 1);// 1 count -> (65536-PCA_START) x 12/22118400 = 20ms }

33

Flowchart & Pseudocode

Figure 2 Flow Chart

34

Pseudo Code:
Globals and function prototypes Main Initialize everything Get a heading and k input from LCD/Keypad Set initial PW of motor to neutral for 1 second start while (1) loop if get a new heading Call Steering function Read ranger Drive fan speed motor Drive functions based off of ranger value Start ping Print the range value port initializations XBR0 init PCA init set 16 bit compare module SYSCLK/12 PCA ISR count up to 40 ms to get new heading ADC_Init read_AD_input do an ADC heading function address of sensor data array heading read (address, start register, data array, number bytes) combine two data values return heading steering function if 40ms has passed call heading function print heading calculate error set PWM value //sets tail fan motor speed

35

Schematic
Car Schematic - Lab 5

Figure 3 La5 Schematic Gondola Schematic Lab 6

Figure 4 La5 6chematic

36