Physical Design and Construction
Total Page：16
File Type：pdf, Size：1020Kb
AwesomeBot
Created by: Andrew Eldredge J. Scott Liljenquist and Bruce Embry
Fall Semester 2004 Mechatronics IT548 Introduction
AwesomeBot is a robot created for the IT548 course at Brigham Young
University. This robot’s mission was to move about autonomously, collecting ping pong balls and returning them to a goal. The following is a description of the development of
AwesomeBot.
Physical Construction and Hardware
In order to minimize weight, AwesomeBot is constructed mainly of .020” AN-A-
12-24 aluminum alloy. Because the base material is so thin, the main chassis was flanged on each side to provide rigidity to the structure. Additional structures were fastened with screws to the flanging so as to be constrained to the rigid structure.
3/8” synchromesh belts were chosen because we found some for free. The early stages of design were spent hampered in large degree in attempts to model sprockets for these belts which could in turn be CNC milled. Although the design of the sprockets was easy to conceptualize, the realization turned out to be difficult than expected for both mechanical engineers on the team, even when experienced outside help was solicited. As a note, we recommend considering alternatives to ProEngineer when selecting a 3D mechanical modeling package to learn. Eventually enough time had been spent on attempts to design sprockets with little progress being made, so we opted to purchase sprockets from the industry leader in mechanical power transmission: Stock Drive
Products. As an alternative to the supplied motors, a pair of Faulhaber series 2224SR motors was used because they were available with attached quadrature encoders. The ball capture mechanism was implemented as a door made out of the same material as the rest of the Robot. Control of the ball catching capture and release mechanism was through a servo connected to the Ferret-Tronics servo controller board.
The board was driven by a serial connection to COM1 of the TS-5500, connected as provided.
AwesomeBot also featured some sweet custom graphics, as seen on the title page.
Motor Control Using the Rabbit 3100 Core Module
The Rabbit 3100 is an 8 bit microcontroller with features which are convenient for controlling dc motors. First, the module is equipped with hardware quadrature decoders, and second, it has hardware dedicated to execute pulse width modulation.
These features are available on several parallel port pins, and are activated by changing the value from default parallel port operation to the alternate motor control features by changing the values in the respective port’s function register. The online documentation for the Rabbit made these features easy to implement by clearly showing which features were available on which ports. Additionally, the addresses of the control and data registers had macros built in to facilitate easier coding and reading of control code.
The command and measurement of the motor speed was further enhanced buy comparing encoder counts to the processor’s real time clock. This way, the speed of each motor was calculated on the Rabbit itself. Because there was still headroom for more processor activity on the Rabbit and because there was code available, motor speed control code was implemented on the Rabbit. The motor velocities were low pass filtered, with the filtered values being used as feedback for a PID controller which in turn drives the pulse width modulation hardware on the core module. The pulse width modulated signals were run to the H-bridge circuits supplied with the TS-5500. The class documentation was unclear on the signal pin assignments for the H-bridge, and after some difficulty, I resorted to the online documentation which clearly explained that in addition to the pins which actually carried the PWM signal, there were “enable” pins which needed to be set high for the bridge to be activated. Alternately, a jumper could be moved for each motor, which kept the motor active at all times.
Communication Between the TS-5500 and the Rabbit 3100
Microcontroller
The Rabbit microcontroller is equipped with six serial ports. These ports can be configured to use standard RS-232 protocol, but they do not operate at standard serial port voltages. Because the processor is a low voltage device, it required a RS-232 to TTL converter in order to interface with the standard serial ports on the TS-5500. Because the servo controller board and the CMU cam were anticipated to occupy the first two ports, attempts were made to connect the rabbit processor to the third and last remaining serial port. This was only achieved with great difficulty. When initial attempts to communicate between the boards failed, a “hello world” application was written for the serial port on the rabbit and connected to Microsoft HyperTerminal in order to ensure that the rabbit was communicating properly. With this working successfully, the serial port parameters were noted, and checked against the parameters specified in the TS-5500 programming. With the receiving port set to wait for received data, the port worked, port was observed to have extremely slow throughput, on the order of five characters every twenty seconds. When all other possibilities seemed exhausted, the operating system of the TS-5500 came under suspicion. With some imported programming help, the troubleshooting commenced. The solution came when two of us were reading the TS-
5500 manual at the same time, one in print and one online. The online version of the manual coincidently contained the saving but obscure piece of information which was not included in the printed manual. In order for COM3 to operate under Linux, a jumper must be set to select the interrupt for the port. When the interrupt is set, it must be registered with the operating system. With that accomplished, the serial port began outputting the hello world message at a fantastic rate. A simple protocol for communicating desired and actual wheel velocities soon followed.
Program Structure
An object oriented software design was selected to facilitate the division of code writing tasks between team members. Each device on the robot was to be represented as an object in the program. Team members were responsible for writing some class, whether for a sensor or actuator interface, then to provide an API to the rest of the team.
Because each team member would be required to write code which interfaced with unfamiliar code, efforts were made to make the programming interfaces have intuitive, real world meaning. Much of the code used in each of these classes was modified from test code or code used in the labs. In each case, the code was modified to be in class structure, with the constructors and destructors of each class establishing the necessary serial or A/D connections and closing each connection as appropriate. The software for the robot contained a main program, along with objects titled agent, platform, camera, catcher, servo, and range finder. A commented version of this code has been included in the appendix.
Program Main:
The main program was never meant to do more than open and instantiate the agent class, much like a GUI program. Once this occurred the agent class was meant to take over all operation in different loops. However, as we developed the software, team members broke off to develop separate portions of the software. While developing separate methods, several different versions of the main function were developed in order to test different functions. It should be noted that while the concept design of this robot was reasonably developed, the programming was not complete before the project came due. While the separate classes were not all completed or integrated at the time of this writing, these descriptions are intended to portray the class structure as it was designed to function in its final state.
Agent Class:
The Agent Class is designed to control all functionality of AwesomeBot. This class holds all other classes as members and coordinates them. Once called by main, The
Agent executes a continuous loop, and is governed in its actions by a state variable.
When this state is set to 0, or “SEARCH”, the robot does not know where the ball is and is commanding speeds and keeping track of its position by communicating with the platform class. After an initial choice of heading and speed, the agent communicates with the range sensor objects in order to fine tune its heading and speed requirements. A differential is taken on the measurements from the range sensors. If the differential is small relative to the robot velocity, the robot should adjust its heading a small amount away from the sensor with the greater differential. If the differential was nearly equal to the velocity of the robot, indicating a wall ahead, the robot should slow down until the distance reached nearly zero, then stop to turn. If the differential is greater than the speed of the robot, then AwesomeBot has detected the presence of an intercepting enemy vessel and erases all vital records in the memory banks before activating the self destruct sequence. This routine did not actually make it out of the concept stage. Also present in the agent object’s loop is a query of the camera object to indicate the presence of a target ball.
After locating a ball and identifying it as good, the state variable is set to 1, or
“CAPTURE.” First, the servo object is commanded to open the door. In this state, the camera object is queried for the position of the camera controlled servo. Heading adjustments are sent to the platform object to bring the camera servo to the center position. Continuing communication with camera, the platform is commanded forward for a set distance beyond the point when the target ball moves from the bottom of the frame. At this point, the servo object is commanded to close the door by moving the servo to the closed position. This method then sets the status flag to 3, or “GOHOME.”
In the “GOHOME” state, the platform object is queried for the current estimated location of the robot. This is compared to a list of waypoints. From any waypoint, a pointer to the next closest waypoint to home is retrieved, a heading is calculated, and the platform object is sent the desired speed and heading. When the estimated position is within a range of the home location, a search is initiated for the home marker, which is a colored ball recognizable by the camera. There the ball is released and the state is set back to “SEARCH.”
Platform:
The main task of the Platform object is to communicate with the Rabbit microcontroller, sending desired speeds and receiving actual speeds. The Platform object is also responsible for keeping track with the estimated position of AwesomeBot, which is performed by numerically integrating the velocities, which are applied through an estimated angle, which is also calculated from measured velocities, to update the estimated position in X and Y.
Camera:
The CMU camera is used to locate balls and identify them by color. This information is received by the camera, and the camera class would determine whether to accept or reject the ball. The original camera code was written in assembly in an attempt to make it universally acceptable to any operating system. It was later adapted to C++ to be more compatible with the rest of the instructions for this project.
The camera contains methods instantiation, calibrate, scan, pan, and found. When the camera class is instantiated it sets up structures to maintain operating variables, and opens communication methods. The communication methods receive information directly from the camera’s onboard processor. One intended mode of operation was to have the camera calibrate on startup with the user holding the target color in front of the camera. The method titled Calibrate takes this color and stores it in a specified structure. The Scan method takes this definition and searches the camera images while the robot is roaming the field. If the defined target is located in a camera image, the Pan method locks the servo control on the position while the Found method returns the information on whether the ball has been located. This information is used by the agent object to increment the state.
Catcher:
The Catcher class was designed to operate the gate that locks the ball for transport. The methods included raise and lower, commanding the gate servo to raise and lower the gate. This class was later replaced by the Servo class as a result of existing code provided to the students.
Servo:
Servo replaced Catcher as an existing file that was implemented by making minor changes to the code provided by Magnus-SWE. Crediting and licensing are included in the title block of the code. The provided code was divided to create the methods for the
Servo class. The methods included the constructor, which set up communication to the
Ferret-Tronics servo controller, “setServo,” and several utility functions which were not used in this project. “SetServo” is also used to raise and lower the gate servo once the target is identified.
Range Finder: Laser range finders work in conjunction with the platform object to navigate AwesomeBot. The higher class, agent, sets up one range finder object for the right side sensor and another for the left. The range finder class simply contains the functions “getDistance” and “distanceDerivative.” “getDistance” acquires a value from the range finders themselves and converts this value to centimeters by multiplying the measurement by a conversion constant. Once this value is computed, the time derivative of this distance is computed and passed to the agent class to be compared with the robot velocity.
Challenges and Possible Solutions
The track driving sprockets were too big. Chosen because the belts available were somewhat rigid, and it seemed like the right thing to do in order to prevent the building in of higher stresses to force the belts into a smaller radius. The proper thing to do would be to measure the incline of the track measure the load due to friction by running the robot on a level surface, then calculate the added power requirements of hauling the mass of the robot up the hill. With the total estimate of the power requirements, the motor can be selected. Each motor has a speed, at which maximum power is transferred, at the very least, when wheel sizes are selected, there is a trade off between motor speed and torque requirements. The wheel size should be such that available torque is not overcome by a large moment arm. Smaller wheels will require the motors to turn faster for a given speed. In the high speed range, the motor is unable to provide adequate torque. When at the extreme of the performance envelope of the motor, wheel selection for optimum power transfer becomes more critical. This is analogous to impedance matching for maximum power transfer in electrical systems. This problem could also be solved by using a gear head with a smaller gear ratio.
Another possibility which could cut down on weight and thereby improve the performance of the robot would be to move all functionality to the Rabbit. The time critical control features are implemented in hardware on the Rabbit, which frees the processor from toggling pins and reading pins all the time, freeing it to check values and make decision on a higher level with greater frequency. Communication would not be a problem with the six serial ports on the Rabbit, a small wireless modem could communicate with the host computer and we have the ports necessary to communicate with the CMU-cam, the servo driver board and numerous other sensors. APPENDIX
As noted in the body of the report, the grand vision described in the report was not implemented in its entirety, in fact, not very much of it was, but this is the code very near to its operating condition on game day.
/************************************************ * Filename:awesome_bot.cpp * * This file is contains the main for running * Awesome Bot. It instantiates the class * "agent" for controlling the robot. While * developing the camera, servo, and motion * control for the mobility platform, class * methods were called from modified versions * of this main function. * * Written by: Andrew Eldredge * Bruce Embry * and J. Scott Liljenquist ************************************************/ #include
// Print statements are included for the purpose of troubleshooting // the servo class. int main() { printf("calling all functions!\n"); cout << "sound off" << endl; agent agent1; cout << "Agent1 reporting for duty!" << endl;
return 1; } /****************************************/ /* Filename:Agent.h /* /* This file defines the character of individual agents /* and controls all of the higher level logic for the robot /* /* Written by: Andrew Eldredge /* Bruce Embry /* and J. Scott Liljenquist /****************************************/
#ifndef AGENT_H #define AGENT_H
#include
void mainLoop(); void positionUpdate(); void search(); void gotoPoint(); void captureBall(); void releaseBall(); void driveMotors();
float getLocationX(); float getLocationY(); float getHeading(); float getDesiredHeading(); float getDesiredSpeed(); float getServoAngle(); private: int state; float locationx, locationy, heading, desiredDistance, desiredHeading, desiredSpeed;
platform rabbit1; camera cam1; catcher catcher1; // rangeFinder rangeRight; // rangeFinder rangeLeft; servo gate; };
#endif /******************************************* * Filename: agent.cpp * * This file controls the agent class. The * majority of all functionallity is called from this class. * * Written by: Andrew Eldredge * Bruce Embry * and J. Scott Liljenquist *******************************************/
#include
// if range is too close change heading, // either way drive forward
// read camera cam1.scan(); */ // if camera sees the ball change the state /* if(state!= cam1.changeState) { state = cam1.changeState;
if (state == 1) { captureBall(); }
} */ // write desired heading and desired speed return; } void agent::gotoPoint() { // write desired heading and speed
return; } void agent::captureBall() { // reduce speed desiredSpeed = 2; desiredDistance = 5;
// open door to recieve ball catcher1.raise();
// lineup on target // set the servo angle to put the ball in the middle of the frame
// creep over ball and stop rabbit1.move(desiredSpeed, desiredDistance);
// shut door to contain ball catcher1.lower();
// Reverse speed and direction desiredSpeed = -4; desiredDistance = -desiredDistance;
// back up to verify ball capture rabbit1.move(desiredSpeed, desiredDistance);
// change state to place ball in home base gotoPoint();
return; } void agent::releaseBall() { return; } void agent::driveMotors() { return; } float agent::getLocationX() { return locationx; } float agent::getLocationY() { return locationy; } float agent::getHeading() { return heading; } float agent::getDesiredHeading() { return desiredHeading; } float agent::getDesiredSpeed() { return desiredSpeed; } float agent::getServoAngle() { return desiredHeading; } /****************************************/ /* Filename: camera.h /* /* This file identifies the methods of /* interfacing with the CMU cam /* /* Written by: Andrew Eldredge /* Bruce Embry /* and J. Scott Liljenquist /****************************************/
#ifndef CAMERA_H #define CAMERA_H class camera { public: camera(); ~camera(){}; int scan(); private: int changeState, filedesc; unsigned char target[10], colors[10]; float relativeAngle, targetAngle; };
#endif /****************************************/ /* Filename: camera.cpp /* /* This file defines the methods of /* interfacing the CMU cam with the PC/104 board /* /* Written by: Andrew Eldredge /* Bruce Embry /* and J. Scott Liljenquist /****************************************/ #include
for(i=1; i<=3; i++) // Calculate color ranges { temp[i] = colors[i] - colors[i+3]; temp[i+3] = colors[i] + colors[i+3]; colors[i] = temp[i]; colors[i+3] = temp[i+3]; } / *------The color ranges are calculated, but in the wrong order: Rmin, Gmin, Bmin, Rmax, Gmax, Bmax. The following will rearrange them to this order: Rmin, Rmax, Gmin, Gmax, Bmin, Bmax. ------*/ colors[2] = temp[4]; // Rmax colors[3] = temp[2]; // Gmin colors[4] = temp[5]; // Gmax colors[5] = temp[3]; // Bmin return; cout << "camera up and running" << endl; } / *------This function will pan the camera continuously, scanning for the target ball ------*/ int camera::scan() { char pos, dir, target[10], temp[10];
write(filedesc, "MM", 4); // Write middle mass command temp[0]=1;temp[1]=6; write(filedesc, temp, 2); write(filedesc, "TC", 2); temp[0]=6; write(filedesc, temp, 1); write(filedesc, colors, 6); // Write tracking colors pos = 64; // Start servo in center dir = -1; // Start panning to left while(!target[8]) { write(filedesc, "S1", 2); // Set servo position temp[0]=1;temp[1]=pos; write(filedesc, temp, 2); write(filedesc, "TC", 2); // continue TC command temp[0]=6; write(filedesc, temp, 1); write(filedesc, colors, 6); read(filedesc, target, 10); // Read packet if(pos == 1) dir = 1; else if(pos == 127) dir = -1; pos += dir; } return (int) target[1]; } /**************************************** * Filename: catcher.h * * This file defines the methods for * interfacing with the catcher servo * It was later replaced by servo.h * * Written by: Andrew Eldredge * Bruce Embry * and J. Scott Liljenquist ****************************************/
#ifndef CATCHER_H #define CATCHER_H class catcher { public: catcher(); ~catcher(){};
void raise(); void lower(); private:
};
#endif /****************************************/ /* Filename: cathcer.cpp /* /* This file identifies the methods for /* interacting with the catcher servo /* The file was scrapped for servo.c /* before developing further /* /* Written by: Andrew Eldredge /* Bruce Embry /* and J. Scott Liljenquist /****************************************/
#include
#ifndef SERVO_H #define SERVO_H
#include
void setServo(int, int); int surveilanceMode(); int manualMode(); void selectMode(int); private: struct termios old_options, new_options;
int device, keyb_servo_number, keyb_servo_position, timez, result, low, high; };
#endif /* * Linux_Servo_FT639 is a Linux servo control program for the FT639 chip. * Copyright (C) 2001, 2002 Magnus-swe
#include
// was known as the open port method in servo.c servo::servo() { /* Set the serial port*/ char serial_port[1024] = "/dev/ttyS1";
const char setup[]={122}; const char shortpulse[]={85}; const char longpulse[]={90}; const char header[]={96}; const char active[]={117}; unsigned char low_nibble =00001111; // send this first unsigned char high_nibble=10000000; // 0000 0011 1011 1111 for servo 0 char bytes_written[]=""; // char serial_port[1024]=""; char control_mode[1024]=""; char center[1024]=""; char input[1024]=""; timez=50000; low=0; high=255;
/* Open the serial port /dev/ttyS0 */ device = open (serial_port, O_RDWR | O_NOCTTY | O_NDELAY); if (device < 0) { printf("\nThe servo class cannot open"); printf(" the serial port %s\n", serial_port); exit(1); }
/* Get the Original port settings */ tcgetattr(device, &old_options);
/* Specify new port settings: BAUDRATE=2400, 8 BITS, No Parity, 1 Stopbit */ new_options.c_cflag = (B2400 | CS8 | CLOCAL); // raw input = no ICANON
/* Specify raw data (deselect some settings) */ new_options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
/* Activate the new port settings */ tcsetattr(device, TCSANOW, &new_options); // TCSAFLUSH
/* Put the chip in setup mode */ write(device, setup, 3); write(device, shortpulse, 2); write(device, longpulse, 2); write(device, header+3, 2);
/* Activate the chip */ write(device, active, 3); printf("\nDevice Activated.\n"); sleep(2); }
// this method sets the servo to a specific value void servo::setServo( int iServo, int iPos ) { unsigned char LowByte, HiByte; LowByte = ((iServo-1) << 4) + (iPos&0x0f); if (write(device, &LowByte, 1)!=1) return; HiByte = ((iServo-1) << 4) + (iPos>>4) + (1 << 7); if (write(device, &HiByte, 1)!=1) return; printf("Low byte = %02X, High Byte = %02X\r\n", LowByte, HiByte); }
int servo::surveilanceMode() { /* --- Surveilance mode --- */ int i; printf("\nChoose Surveilance Servo: 1 to 5\n"); scanf("%s", &keyb_servo_number);
printf("\n\nStarting in SurveilanceMode...\n"); printf("\nPress CTRL+C to exit\n");
while(1) { /* Endless loop */ for(i=low;i
// This method has no use in an autonomous vehicle int servo::manualMode() { int step; char char_keyb_servo_position[1024]=""; printf("\n\nStarting in ManualMode...\n"); printf("\nPress CTRL+C to exit\n");
printf("\nChoose manual Servo: 1 to 5\n"); scanf("%s", &keyb_servo_number);
while(1) { printf("\nChoose Position to step to: 0 to 255\n");
scanf("%s", &char_keyb_servo_position); step=atoi(char_keyb_servo_position);
setServo(keyb_servo_number, step); usleep(50000); } }
/* This section is only useful where it sets up the surveillance mode int main (int argc, char *argv[]) { */ /* Select Serial Port * printf("Choose Serial Port IE: /dev/ttyS0\n"); scanf("%s", &serial_port); open_port(); */ void servo::selectMode(int controlMode) { /* printf("\nChoose Surveilance_Mode[1] or Manual_Mode[2]:\n"); scanf("%s", &control_mode); */ // if (strstr(control_mode, "1")) if (controlMode==1) { printf("\nStarting in surveilance mode\n");
/* setting default scanrange values */ low=0; high=255; /* Center all servos */ setServo(1, 130); /* setServo(2, 130); setServo(3, 130); setServo(4, 130); setServo(5, 130);
/* printf("Select scan range (default is 0-255) ? [y/n]\n"); scanf("%s", &input); if(strstr(input, "y")) { printf("Select LOW scan range value [0 to 200]:\n"); scanf("%s", &input); low=atoi(input); if(low>200) { printf("Using low scanrange: 0\n"); low=0; } printf("Select HIGH scan range value [0 to 255]:\n"); scanf("%s", &input); high=atoi(input); if(high>255) { printf("Using high scanrange: 255\n"); high=255; } } */ surveilanceMode(); } else // if (strstr(control_mode, "2")) if (controlMode == 2) { printf("\nStarting in manual mode\n"); /* Set all servos to the center position first */
/* printf("\nDo you want to center all servos ? [y/n]:\n"); scanf("%s", ¢er); if(strstr(center, "y")) { SetServo(1, 130); SetServo(2, 130); SetServo(3, 130); SetServo(4, 130); SetServo(5, 130); } usleep(50000); manual_mode(); } else printf("\nThe choises are: 1 or 2\n");
/* Put the chip back in setup mode (for "real" applications) */ printf("\n\nEntering SetupMode and closing serial line\n");
const char setup[]={122}; write(device, setup, 4);
/* Put the Original port settings back */ tcsetattr(device, TCSANOW, &old_options); // TCSAFLUSH TCSANOW close(device); return; } } /******************************************/ /* Filename: Platform.h */ /* */ /* This file identifies the methods of */ /* interfacing with the rabbit */ /* */ /* Written by: Andrew Eldredge */ /* Bruce Embry */ /* and J. Scott Liljenquist */ /******************************************/
#ifndef PLATFORM_H #define PLATFORM_H
#define BASE #define WHEEL_RADIUS class platform { public: platform(); ~platform(){};
void setSpeed(float); void setOmega(float); float readSpeed(); float readOmega(); float readPositionX(); float readPositionY(); float readHeading(); void flagPosition(); void centerCamera(float); private: struct termios old_options, new_options;
int com3; int timez=10; char stuff[64]; char sendthis[64]; int error=0; float vdesleft, vdesright; float vactleft, vactright;
float speed, omega, positionx, positiony, heading, desiredPositionX, desiredPositionY, desiredHeading; };
#endif
/****************************************/ /* Filename:Platform.cpp */ /* */ /* This file controls the interaction */ /* with the rabbit processor */ /* */ /* Written by: Andrew Eldredge */ /* Bruce Embry */ /* and J. Scott Liljenquist */ /****************************************/
#include
// open serial port 2, for read & write | not controlling | dont // wait for data carrier detect line DCD // com3 = open ("/dev/ttyS2", O_RDWR | O_NOCTTY | O_NDELAY); com3 = open ("/dev/ttyS2", O_RDWR | O_NOCTTY); if (com3 < 0) { printf("\nCant open the serial port\n"); exit(1); } /* Get the Original port settings */ tcgetattr(com3, &old_options); new_options = old_options;
//cfgetispeed(&new_options, B19200); //cfgetospeed(&new_options, B19200);
/* New port settings: BAUDRATE=19200, 8 BITS, No Parity, 1 Stopbit */ //new_options.c_cflag = (B19200 | CS8 | CLOCAL); // raw input = no ICANON
new_options.c_cflag |= (B19200 | CLOCAL); new_options.c_cflag &= ~CSTOPB; new_options.c_cflag &= ~PARENB; new_options.c_cflag &= ~CSIZE; new_options.c_cflag |= CS8; new_options.c_cflag &= ~CRTSCTS;
/* Specify raw data (deselect some settings) */ new_options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG | ECHOK | ECHOCTL | ECHOE); new_options.c_iflag &= ~(IXON | IXOFF | IXANY | ICRNL);
new_options.c_oflag &= ~OPOST;
/* Activate the new port settings */ error = tcflush(com3, TCIFLUSH); error = tcsetattr(com3, TCSANOW, &new_options); // TCSAFLUSH error = tcflush(com3, TCIFLUSH);
return; }
for(int i = 0 ; i < timez ; i++) {
for (int j = 0; j < 64; j++) stuff[j] = '\0'; sprintf(stuff, ""); int nbytesread = read(com3,stuff,8);
memcpy(&vactleft, &stuff[0],4); memcpy(&vactright,&stuff[4],4);
printf("vleft: %8.4f\t vright: %8.4f\n", vactleft, vactright); if (error < 0) { printf("error code: %d\n", errno); } //usleep(200000); }
tcsetattr(com3, TCSANOW, &old_options); //reset com port to old settings close(com3);
void platform::setSpeed(float newSpeed) { memcpy(&sendthis[0],&vdesleft, 4); memcpy(&sendthis[4],&vdesright,4); write(com3,sendthis,8); speed = newSpeed; return; } void platform::setOmega(float newOmega) { omega = newOmega; return; } float platform::readSpeed() { return speed; } float platform::readOmega() { return omega; } float platform::readPositionX() { return positionx; } float platform::readPositionY() { return positiony; } float platform::readHeading() { return heading; } void platform::flagPosition() { return; } void platform::centerCamera(float headingGoal) { desiredHeading = headingGoal; // this should have a delta angle, or calculate desired // then drive the wheels while not there return;
} /**************************************** * Filename: rangeFinder.h * * * * This file identifies the methods of * * interfacing with the range finders * * from the PC/104 board * * * * Written by: Andrew Eldredge * * Bruce Embry * * and J. Scott Liljenquist * ****************************************/
#ifndef RFINDER_H #define RFINDER_H class rangeFinder { public: rangeFinder(); ~rangeFinder(){};
float getDistance(); float distanceDerivative(); private: float time[3], sumTime, distance, oldDistance[3], rangeDerivative, scale; };
#endif /**************************************************** * Filename: rangeFinder.cpp * * * * This file defines the methods of interfacing * * with the range finders. Unfortunately the * * communication with the range finders was never * * effectively established in this project. Many * * of the methods are waiting for something to * * interface with. * * * * Written by: Andrew Eldredge * * Bruce Embry * * and J. Scott Liljenquist * ****************************************************/
#include
// the code is designed to provide a reaction to the derivative // over time. Thus if the distance between the robot and the object // being detected is not changing with time then it is assumed that // the robot is moving in a parallel path to the object and only needs // to maintain a reasonable distance. If the derivatve is negative. // the robot does not need to react at all. A small positive derivative // will cause a minor course correction. A large positive derivative is // meant to cause the robot to stop and make a sharp course correction // before moving forward.
// these methods are left for future projects } / *********************************************************************** ************* Motordriver.c - Source code for PID control of two motrors with the Rabbit Core Module 3100
Portinons of this code are modified from code written in the BYU MAGICC LAB by Walter Johnson
Modification list 26-Jan-04 whj 1.00 Library created.
*********************************************************************** **************/
/***************************************************** * Definitions ******************************************************/ #define CLK_SEC 32768.0 // Clock cycles per second (29MHz rabbit) #define SEC_CLK 0.000030517578 // Seconds per clock cycle (29MHz rabbit)
#define QUAD_ISR_ENABLE() WrPortI ( QDCR, &QDCRShadow, 0xCE ) #define QUAD_ISR_DISABLE() WrPortI ( QDCR, &QDCRShadow, 0x00 )
#define QUAD_0_COUNT() RdPortI(QDC1R) #define QUAD_1_COUNT() RdPortI(QDC2R)
// CMD = PE0, CLK = PE3, DATA = PE5 #define SPI_INIT() WrPortI(PEDDR, &PEDDRShadow, 0x28) // set PE3,PE5 as output #define SPI_READ_CMD() BitRdPortI(PEDR, 0) #define SPI_CLK_HIGH() BitWrPortI(PEDR, &PEDRShadow, 1, 3) #define SPI_CLK_LOW() BitWrPortI(PEDR, &PEDRShadow, 0, 3) #define SPI_CLK_PULSE() SPI_CLK_LOW();SPI_CLK_HIGH();SPI_CLK_LOW() #define SPI_DATA_HIGH() BitWrPortI(PEDR, &PEDRShadow, 1, 5) #define SPI_DATA_LOW() BitWrPortI(PEDR, &PEDRShadow, 0, 5)
/* This goes into the PC (reciever client) */ #define SPI_READ_CLK() BitRdPortI(PEDR, 0) // replace with appropriate pin control functions #define SPI_READ_DATA() BitRdPortI(PEDR, 0) // replace with appropriate pin control functions #define PKTSIZE 9 // two floats #define CINBUFSIZE 63 #define COUTBUFSIZE 127
#define COUNTS_TO_CM 0.00073017
/****************************************************** * Global Variables ******************************************************/ struct quad_stuff { long rtc; long rtc_last; long ctr; long ctr_last; int overflow_ctr; float vel; } quad[2]; typedef struct LPF_block { float *source_ptr; float ang_result; float result; float corner_freq; unsigned long rtc; unsigned long rtc_prev; }LPF_block; typedef struct sat_block { float *source_ptr; float result; float limit_lower; float limit_upper; }sat_block; typedef struct PID_block // PID uses low- pass filter on derivative input { float Kp; // gain: Proportional float Ki; // gain: Integral float Kd; // gain: Derivative sat_block effort_sat; // effort saturation level
float *desired_ptr; // Desired effort pointer float *actual_ptr; // Actual effort pointer float error; // error in signal float error_prev; // previous error in signal float effort_integ; // Integral control effort float effort; // Control effort
unsigned long rtc; // Real- time clock (RTC) value unsigned long rtc_prev; // RTC previous value }PID_block;
/****************************************************** * Prototypes ******************************************************/
//Quadrature Read Stuff //lines nodebug long rtc_read(); void setup_quadrature(void); nodebug root interrupt void quadrature_isr();
//Motor Driving Functions void setup_pwmod(void); nodebug unsigned long motor_init(unsigned long frequency); nodebug int motor_set(int channel, int duty_cycle, int options);
//PID Calculation Stuff void LPF_init(LPF_block *pp, float *source_ptr, float corner_freq); float LPF_calc(LPF_block *pp); void sat_init(sat_block *pp, float *source_ptr, float limit_lower, float limit_upper); float sat_calc(sat_block *pp); float sat_calc_simple(float *val, float limit_lower, float limit_upper); void PID_init(PID_block *pp, float *desired_ptr, float *actual_ptr, float Kp, float Ki, float Kd, float effort_min, float effort_max); float PID_calc(PID_block *pp);
//Communication Stuff //serial_init(what port, what speed); //serial_read(); //serial_write();
//Not using a SPI bus at this time, but these are down there //void send_data(); //void send_byte(unsigned char data);
/ ************************************************************************ * MAIN
***********************************************************************/ void main() { int i; long dt, dist; LPF_block lpfLeft; LPF_block lpfRight; PID_block pidLeft; PID_block pidRight; float increment; float avel_left, dvel_left; float avel_right, dvel_right; char s[16];
/* INITIALIZATION */ setup_quadrature();
motor_init(5000); //this frequency argument is circumvented to be very fast
increment = 0.2; dvel_left = dvel_right = 0; avel_left = avel_right = 0;
LPF_init(&lpfLeft, &avel_left, 18.5); LPF_init(&lpfRight, &avel_right, 18.5); PID_init(&pidLeft, &dvel_left, &lpfLeft.result, 512.0, 1000.0, 0.07, -1024, 1024); PID_init(&pidRight, &dvel_right, &lpfRight.result, 512.0, 1000.0, 0.07, -1024, 1024);
//SPI_INIT();
i = 0; memset(quad, 0, 2*sizeof(struct quad_stuff));
serCopen(19200);
loopinit(); // used for costates while(1) { loophead();
/* Update Quadrature Info */ costate { waitfor(DelayMs(5)); // This could be 10 ms
/* Quadrature 1 */ quad[0].rtc = rtc_read(); quad[0].ctr = QUAD_0_COUNT(); quad[0].ctr |= ((long)quad[0].overflow_ctr)<<8;
dt = quad[0].rtc-quad[0].rtc_last; dist = quad[0].ctr-quad[0].ctr_last;
quad[0].vel = (float)dist/(float)dt;
/* Update Memory */ quad[0].rtc_last = quad[0].rtc; quad[0].ctr_last = quad[0].ctr;
/* Quadrature 2 */ quad[1].rtc = rtc_read(); quad[1].ctr = QUAD_1_COUNT(); quad[1].ctr |= ((long)quad[1].overflow_ctr)<<8;
dt = quad[1].rtc-quad[1].rtc_last; dist = quad[1].ctr-quad[1].ctr_last;
quad[1].vel = (float)dist/(float)dt;
/* Update Memory */ quad[1].rtc_last = quad[1].rtc; quad[1].ctr_last = quad[1].ctr;
/* Calculate effort */ avel_left = quad[0].vel; avel_right = -1*quad[1].vel;
LPF_calc(&lpfLeft); // low-pass filter velocity LPF_calc(&lpfRight); PID_calc(&pidLeft); // calculate PID feedback PID_calc(&pidRight);
/* Set Motor effort */ if(pidLeft.effort >= 0) { motor_set(0,0,0x01); motor_set(1,(int)pidLeft.effort,0x01); } else { motor_set(0,(int)abs(pidLeft.effort),0x01); motor_set(1,0,0x01); }
if(pidRight.effort >= 0) { motor_set(3,0,0x01); motor_set(2,(int)pidRight.effort,0x01); } else { motor_set(3,(int)abs(pidRight.effort),0x01); motor_set(2,0,0x01); } //motor_set(0,0 ,0x01); //Left Wheel, backward //motor_set(1,1024 ,0x01); //Left Wheel, foreward //motor_set(2,800 ,0x01); //Right Wheel, foreward //motor_set(3,0,0x01); //Right Wheel, backward }
/* Print Quadrature/PID Info */ costate { waitfor(DelayMs(100));
printf("L des: %8.4f L err: %8.4f \t R des: %8.4f R err: %8.4f\n", dvel_left, pidLeft.error, dvel_right, pidRight.error); printf("L effort: %8.4f \tR err: %8.4f\n\n", pidLeft.effort, pidRight.effort); //printf("ctr %4d: %8ld %8.4f %8ld %8.4f\n", i++, // quad[0].ctr, quad[0].vel, quad[1].ctr, quad[1].vel ); }
/* Update Desired speeds from the serial connection */ costate { waitfor(serCread(s, 9, 20)); //check packet type, perform correct operation if(s[0] == 0x02) { memcpy( &dvel_left , &s[1] , 4 ); memcpy( &dvel_right , &s[5] , 4); //printf( "%8.4f\t%8.4f\n" , dvel_left , dvel_right); }
if(s[0] == 0x04) { memcpy( &s[1], &avel_left , 4 ); memcpy( &s[5], &avel_right, 4); //printf( "Sending: %8.4f\t%8.4f\n" , avel_left , avel_right); serCwrite(s, 9); } } /* costate { waitfor(DelayMs(200));
if(dvel_left > 3.5 || dvel_left < -3.5) { increment = -1*increment; } dvel_left += increment; dvel_right += increment;
}*/
//if(SPI_READ_CMD()) // send_data(); } }/*********************************end Main()***************************/
/ ************************************************************************ * These Functions are for reading Quadrature Encoders
***********************************************************************/ nodebug long rtc_read() { long rtc_val; WrPortI(RTC0R, NULL, 0xFF);
rtc_val = ((long)RdPortI(RTC0R)); rtc_val |= (((long)RdPortI(RTC1R))<<8); rtc_val |= (((long)RdPortI(RTC2R))<<16); rtc_val |= (((long)RdPortI(RTC3R))<<24);
return rtc_val; } void setup_quadrature() { // SetVectIntern(0x1A, capture_isr); // Input capture SetVectIntern(0x19, quadrature_isr); // Setting interrupt vector, (I think) WrPortI ( QDCSR, &QDCSRShadow, 0x11 ); // Reset Q1 and Q2 counters WrPortI ( QDCR, &QDCRShadow, 0x8B ); // PF0-PF3 as inputs, interrupt prior. 3 // WrPortI ( QDCR, &QDCRShadow, 0xCF ); // PF4-PF7 as inputs, interrupt prior. 3 // WrPortI ( QDCR, &QDCRShadow, 0x0F ); // PF4-PF7 as inputs, interrupt prior. 1 // WrPortI ( QDCR, &QDCRShadow, 0xC3 ); // PF4-PF7 as inputs, interrupt prior. 1 } nodebug root interrupt void quadrature_isr() {
switch(RdPortI(QDCSR)) { case 0x04: // Q1 wrapped 0x00 -> 0xFF quad[0].overflow_ctr++; break; case 0x08: // Q1 wrapped 0xFF -> 0x00 quad[0].overflow_ctr--; break; case 0x40: // Q2 wrapped 0x00 -> 0xFF quad[1].overflow_ctr++; break; case 0x80: // Q2 wrapped 0xFF -> 0x00 quad[1].overflow_ctr--; break; }
}/*************************End of Quadrature Read Functions***************/
/ ************************************************************************ * These Functions are for writing to PWM registers to control the motors
***********************************************************************/ void setup_pwmod() { WrPortI ( PFDDR, &PFDDRShadow, 0xF0); // PF4-PF7 as outputs WrPortI ( PFFR, &PFFRShadow, 0xF0); // PF4-PF7 as pulse width mod (pwm0-pwm3) }
/* As you can see, I have fiddled with this function...AME servo_init (below) uses the BIOS variable 'freq_divider' to determine how fast the processor is running. freq_divider is equal to the value that would be placed in a baud timer for a serial port for 19200 baud operation. Since the baud clock is 16 counts per bit and there are 1024 counts in a PWM cycle, a base frequency corresponding to a 0 value in the PWM prescale timer(TA9) can be calculated: 19200*16/1024 = 300 */ #define PWM_FREQ_FACTOR 300 /*** EndHeader */
/* START FUNCTION DESCRIPTION ******************************************** servo_init
SYNTAX: unsigned long servo_init(unsigned long frequency);
DESCRIPTION: Sets the base frequency for the PWM pulses and enables the PWM driver on all four channels. The base frequency is the frequency without pulse spreading. Pulse spreading(see pwm_set) will increase the frequency by a factor of 4.
PARAMETER1: frequency(in Hz)
RETURN VALUE: actual frequency set. This will be the closest possible match to the requested frequency.
END DESCRIPTION **********************************************************/ nodebug unsigned long motor_init(unsigned long frequency) { auto unsigned long divisor; auto unsigned long base_frequency;
base_frequency = (long)freq_divider * (long)PWM_FREQ_FACTOR; divisor = base_frequency/frequency - 1; //divisor =1; if(divisor < 0) { divisor = 0; } else if(divisor > 255) { divisor = 255; } WrPortI(TAT9R, &TAT9RShadow, (char)divisor); if(divisor > 0) { return base_frequency/(divisor+1); } else { return base_frequency; } }
#define PWM_SPREAD 0x01 #define PWM_OPENDRAIN 0x02 /*** EndHeader */ /* START FUNCTION DESCRIPTION ******************************************** motor_set
SYNTAX: int servo_set(int channel, int duty_cycle, int options);
DESCRIPTION: Sets a duty cycle for one of the PWM channels. The duty cycle can be a value from 0 to 1024, where 0 is logic low the whole time, and 1024 is logic high the whole time. Option flags are used to enable features on an individual PWM channel. Bit masks for these are: PWM_SPREAD - sets pulse spreading. The duty cycle is spread over four seperate pulses to increase the pulse frequency. PWM_OPENDRAIN - sets the PWM output pin to be open-drain instead of a normal push-pull logic output.
PARAMETER1: channel - PWM channel(0 to 3) or CH1 = 0, CH2 = 1, CH3 = 2, CH4 = 3
PARAMETER2: duty_cycle - value from 0 to 1024 PARAMETER3: options - combination of optional flags(see above)
RETURN VALUE: 0 if ok -1 if an invalid channel number is used -2 an invalid duty_cycle was requested
END DESCRIPTION **********************************************************/ nodebug int motor_set(int channel, int duty_cycle, int options) { auto int channel_bit; auto char lsb_reg, msb_reg;
if(channel < 0 || channel > 3) { return -1; } if(duty_cycle < 0 || duty_cycle > 1024) { return -2; } if(duty_cycle == 0) { BitWrPortI(PFDR, &PFDRShadow, 0, channel + 4); //always low BitWrPortI(PFFR, &PFFRShadow, 0, channel + 4); //normal output } else { duty_cycle--; //counts are n+1 lsb_reg = duty_cycle << 6; //shift for setting msb_reg = duty_cycle >> 2; if(options & PWM_SPREAD) { lsb_reg |= 0x01; //prep spread flag } WrPortI(PWM0R + 2*channel, NULL, msb_reg); WrPortI(PWL0R + 2*channel, NULL, lsb_reg); BitWrPortI(PFFR, &PFFRShadow, 1, channel + 4); //PWM output }
if(options & PWM_OPENDRAIN) { BitWrPortI(PFDCR, &PFDCRShadow, 1, channel + 4); } else { BitWrPortI(PFDCR, &PFDCRShadow, 0, channel + 4); // push-pull output } BitWrPortI(PFDDR, &PFDDRShadow, 1, channel + 4); //enable output return 0; }/*******************End of Motor Control Functions*********************/
/ ************************************************************************ * These Functions are for calculating control efforts, including a filter
***********************************************************************/
/* START FUNCTION DESCRIPTION ******************************************** LPF_init
SYNTAX: void LPF_init(LPF_block *pp, float *source_ptr, . float corner_freq); DESCRIPTION: Used to initialize a Low-pass & High-pass filter block.
END DESCRIPTION **********************************************************/ void LPF_init(LPF_block *pp, float *source_ptr, float corner_freq) { pp->source_ptr = source_ptr; pp->corner_freq = corner_freq;
pp->rtc = 0; pp->rtc_prev = 0; pp->result = 0; pp->ang_result = 0; }
/* START FUNCTION DESCRIPTION ******************************************** calc_integ
SYNTAX: float LPF_calc(LPF_block *pp); DESCRIPTION: Used to calculate Low-pass filter values.
END DESCRIPTION **********************************************************/ float LPF_calc(LPF_block *pp) { float tmp, alpha, dt;
/* Update History */ pp->rtc = rtc_read(); dt = ((float)(pp->rtc - pp->rtc_prev))*SEC_CLK; pp->rtc_prev = pp->rtc;
/* Find LPF alpha */ tmp = dt * pp->corner_freq; alpha = tmp/(1+tmp);
/* Find Low-Pass Filtered Values */ return pp->result = alpha*(*pp->source_ptr) + (1-alpha)*pp- >result; }
/* START FUNCTION DESCRIPTION ******************************************** sat_init
SYNTAX: void sat_init(sat_block *pp, float *source_ptr, . float limit_lower, float limit_upper); DESCRIPTION: Initialize a derivate block.
END DESCRIPTION **********************************************************/ void sat_init(sat_block *pp, float *source_ptr, float limit_lower, float limit_upper) { pp->source_ptr = source_ptr; pp->result = 0;
pp->limit_lower = limit_lower; pp->limit_upper = limit_upper; }
/* START FUNCTION DESCRIPTION ******************************************** sat_calc
SYNTAX: float sat_calc(sat_block *pp); DESCRIPTION: Saturation block. END DESCRIPTION **********************************************************/ float sat_calc(sat_block *pp) { if(*pp->source_ptr < pp->limit_lower) return pp->result = pp->limit_lower;
if(*pp->source_ptr > pp->limit_upper) return pp->result = pp->limit_upper;
return *pp->source_ptr; }
/* START FUNCTION DESCRIPTION ******************************************** sat_calc_simple
SYNTAX: float sat_calc_simple(float *val, float limit_lower, float limit_upper); DESCRIPTION: Saturation block that doesn't use the sat_block sturcture.
END DESCRIPTION **********************************************************/ float sat_calc_simple(float *val, float limit_lower, float limit_upper) { if(*val < limit_lower) return *val = limit_lower; if(*val > limit_upper) return *val = limit_upper;
return *val; }
/* START FUNCTION DESCRIPTION ******************************************** PID_init
SYNTAX: void PID_init(PID_block *pp, float *desired_ptr, float *actual_ptr, . float Kp, float Ki, float Kd, float effort_min, float effort_max);
DESCRIPTION: Initalizes PID_block structure. This PID has an effort limit (min & max) and anti-integrator windup.
PARAMETER1: PID_block structure pointer. PARAMETER2: Desired value pointer. PARAMETER3: Actual value pointer. PARAMETER4: Proprotional gain. PARAMETER5: Integral gain. PARAMETER6: Derivative gain. PARAMETER7: Minimum effort. PARAMETER8: Maximum effort.
END DESCRIPTION **********************************************************/ void PID_init(PID_block *pp, float *desired_ptr, float *actual_ptr, float Kp, float Ki, float Kd, float effort_min, float effort_max) { pp->Kp = Kp; pp->Ki = Ki; pp->Kd = Kd;
sat_init(&pp->effort_sat, &pp->effort, effort_min, effort_max);
pp->desired_ptr = desired_ptr; pp->actual_ptr = actual_ptr;
pp->error = 0; pp->error_prev = 0; pp->effort_integ = 0; pp->effort = 0;
pp->rtc = rtc_read(); pp->rtc_prev = pp->rtc; }
/* START FUNCTION DESCRIPTION ******************************************** PID_calc
SYNTAX: float PID_calc(PID_block *pp);
DESCRIPTION: Used to calculate PID control efforts and internal state values. The desired and actual values are pointers and the effort is a variable of the PID_block structure. This PID has an effort limit (min & max) and anti-integrator windup.
PARAMETER: Pointer to the PID_block structure to be calculated.
RETURN VALUE: The control effort.
END DESCRIPTION **********************************************************/ float PID_calc(PID_block *pp) { float effort_prop, effort_deriv; float dt, d_error, d_alpha;
/* Find Time-Step */ pp->rtc = rtc_read(); // Update time info dt = ((float)(pp->rtc - pp->rtc_prev))*SEC_CLK; pp->rtc_prev = pp->rtc; pp->error = *pp->desired_ptr - *pp->actual_ptr; // Find error in signal
/* Proportional */ effort_prop = pp->Kp * pp->error;
/* Integregal */ pp->effort_integ += pp->Ki * (pp->error*dt); sat_calc_simple(&pp->effort_integ, pp->effort_sat.limit_lower, pp->effort_sat.limit_upper);
/* Derivate */ d_error = pp->error - pp->error_prev; if(dt == 0) effort_deriv = 0; // don't divide by zero else effort_deriv = pp->Kd * d_error / dt;
/* Update History */ pp->error_prev = pp->error;
/* Find Total Control Effort */ pp->effort = effort_prop + pp->effort_integ + effort_deriv; return pp->effort = sat_calc(&pp->effort_sat); // Effort saturation }
/*********************************************************************** * These Functions are for communicating via a direct serial connection
***********************************************************************/ //serial_init(what port, what speed); //serial_read(); //serial_write(); /*
/*********************************************************************** * These Functions are for communicating via a direct SPI bus (one way)
***********************************************************************/ /* void send_data() { unsigned char pkt[PKTSIZE]; int i,j; for (j = 0 ; j < 200 ; j++);
//quad[0].vel =0.0; //quad[1].vel =3.1415926; memcpy(&pkt[0], &quad[0].vel, sizeof(float)); memcpy(&pkt[4], &quad[1].vel, sizeof(float));
for(i=0; i /* void send_byte(unsigned char data) { int i,j; //printf("Databyte : %02X\n", (unsigned)data); for(i=0; i<8; i++) { if(data & 0x01) SPI_DATA_HIGH(); else SPI_DATA_LOW(); SPI_CLK_HIGH(); for(j=0 ; j<120; j++); SPI_CLK_LOW(); for(j=0 ; j<120; j++); //SPI_CLK_PULSE(); data >>= 1; } } /* Returns 1 if sucessful */ /*int read_data() { unsigned char pkt[PKTSIZE]; int i; for(i=0; i return 1; }*/ /* Returns 1 if sucessful */ /*int read_byte(unsigned char *data) { int timer; int i; timer = 0; *data = 0; for(i=0; i<8; i++) { while(!SPI_READ_CLK()) // wait for CLK rising edge or timeout if(timer++>5000) return 0; if(SPI_READ_DATA()) *data |= 0x80; *data >>= 1; } return 1; } */