https://hades.mech.northwestern.edu//api.php?action=feedcontributions&user=Sam+Bobb&feedformat=atomMech - User contributions [en]2024-03-28T10:43:56ZUser contributionsMediaWiki 1.35.9https://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_E-puck_Code_(outdated)&diff=18229RGB Swarm Robot Project E-puck Code (outdated)2010-04-09T19:30:39Z<p>Sam Bobb: </p>
<hr />
<div>This page documents the e-puck code for the RGB Sensing Swarm Robotics project. The code on the e-puck was written in C and compiled using Microchip's MPLAB C Compiler for dsPIC DSCs (student version). <br />
<br />
This code is a branch of the [[Swarm Project E-puck Code]].<br />
<br />
For a high level overview, review the [[Media:Swarm-robotics-flow-chart-epuck-all.pdf|flow chart]].<br />
<br />
==Tasks==<br />
<br />
===Complete===<br />
* Got ADC working for color sensor<br />
** Set up filtering to address projector PWM<br />
* Restructured code to make more modular.<br />
** Split dsPIC_XBeePackets and wheel_speed_coordinator into h and c files<br />
** Pulled packet assembling code out of main and created send_packet() function in send_packet.h/c.<br />
** Pulled a bunch of variables and defines (''NUM_DATA_SETS, NUMBERS_PER_SET, DATATYPE_BYTELENGTH , DATA_ARRAY_LENGTH , ADDITIONAL_NUMS, notRTS, T1_INT_FLAG, x_i, u_i, w_i, x_sum, w_sum, MAX_WHEEL_V_TICKS, deadband, COMMR, SAFEDIST, MINDIST, u_x_ideal, u_y_ideal, x_motion_integral, y_motion_integral, SQUARE'') that were scattered across h files into global_vars.h/c. Makes it easy to include them in a particular file with the ''extern'' keyword.<br />
* Added color_cal() function in color_cal.h/c<br />
* Added wheelSpeedSingleBot to wheel_speed_coordinator<br />
<br />
===To Do===<br />
* Finish color calibration (color_cal) program, to facilitate simple calibration for each e-puck that does not require large amount of data collected for each run, and with minimal user input/control (desire automation and simplification)<br />
* Improve the vision system position information updater<br />
** So that the e-puck does not automatically assume every packet from the Vision System is true, rather 'checks' for accuracy<br />
* Replace wheelSpeedSingleBot with the three step move controller from NUtest.c<br />
**Implement this so that the e-puck can be moved with position and orientation (this is ideal so that data from every orientation of the e-puck can be recorded while calibrating or collecting data<br />
* Implement new algorithm from paper<br />
<br />
==Project Package==<br />
The source code for the project is available here:<br />
*[[Media:RGB_Swarm_Puck_Code_working_version.zip|'''RGB Swarm Puck Code working version.zip''']]<br />
**Open swarm_epucks.mcw and you should be good to go.<br />
<br />
==Description of the files and functions==<br />
<br />
===global_vars(.c/.h)===<br />
* .c/.h: declare and define global variables and macros<br />
====Packet Length Constants====<br />
These variables determine the length of the XBee packets. See [[Swarm_Robot_Project_Documentation#Data_Frame|Data Frame]] and the section on XBee API packets in the XBee manual for further clarification.<br />
<br />
Much of this is still sending integral consensus estimator data. This can be removed or replaces with data needed for decentralized color sensing. <br />
<br />
=====#define NUM_DATA_SETS 5 ===== <br />
Number of statistics on which you are running the consensus estimator. This this particular case, 5. (Ix, Iy, Ixx, Ixy, Iyy)<br />
<br />
=====#define NUMBERS_PER_SET 2=====<br />
Number of variables in each data set (see above) that the consensus estimator needs to transmit to other agents. In this case, 2 because there is <tt>x_i</tt> and <tt>w_i</tt> for each statistic.<br />
<br />
=====#define DATATYPE_BYTELENGTH 4 =====<br />
Number of bytes in the data type (used in the consensus estimator (float = 4 bytes long). This is important because we need to split the numbers into individual bytes to be able to send them out the serial port.<br />
<br />
=====#define DATA_ARRAY_LENGTH (NUM_DATA_SETS*NUMBERS_PER_SET)=====<br />
Total number of data variables needed for the consensus estimator. In this case, it is 5*2=10.<br />
<br />
=====#define ADDITIONAL_NUMS 8=====<br />
Additional number of data to be appended to data array. It is 5 in this case, so that we can append <br />
# Robot X coordinate<br />
# Robot Y coordinate<br />
# Robot Theta orientation<br />
# Robot left wheel speed<br />
# Robot right wheel speed<br />
# ''Sensor Red Value''<br />
# ''Sensor Green Value''<br />
# ''Sensor Blue Value''<br />
<br />
===main(.c/.h)===<br />
* .c: This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines. <br />
* .h: Contains variables, function prototypes, and delay function needed for main.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void)=====<br />
Timer1 ISR. Sets T1_INT_FLAG which provides timing for the main loop.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)=====<br />
UART2 receive ISR. Runs with the XBee receives packets. The switch/case structure handles the packets.<br />
<br />
The current handling of coordinate data from the vision system: <br />
<br />
<code><pre><br />
case 0: //coordinate data<br />
e_set_configuration(<br />
InPacket.data[1].dataFloat,<br />
InPacket.data[2].dataFloat, <br />
InPacket.data[3].dataFloat);<br />
break;<br />
</pre></code><br />
<br />
=====TO DO=====<br />
Needs to be improved. Suggested new function:<br />
* Get vision system data<br />
* Check if it's wildly off from current puck estimate<br />
** If not, update, clear log of rejected packets<br />
** If so, reject and log<br />
*** If we have rejected enough packets (some threshold) and they've all been in similar place (within tolerance), we can assume the puck is wrong and vision system is right. In this case, update with vision system data and clear rejected data log.<br />
<br />
=====int main(void)=====<br />
Setup functions and main loop.<br />
<br />
===color_cal(.c/.h)===<br />
* .c: Contains void calibrate_color(void) function to run the calibration routine.<br />
* .h: Contains function prototype and constant definitions for calibrate_color.<br />
<br />
=====void calibrate_color(void)=====<br />
This function runs the color calibration routine. Eventually this should be turned into a separate e-puck command from the vision system. Currently it just runs the puck through a zig zag pattern on the floor and sends packets. You can collect these with the data logger and process them in matlab. From this, you should be able to get a calibration function.<br />
<br />
=====TO DO=====<br />
* Add to the pattern. Need to collect more data.<br />
* Possibly have the puck store data, find a best fit, and create the calibration function on board.<br />
* Store the calibration function in the EEPROM (flash memory) so it's non-volatile. The coefficients of function will be specific to each puck, so it would be nice to not have to program each puck, each time you change the battery.<br />
<br />
===dsPIC_XBeePackets(.c/.h)===<br />
* .c/.h: Contains functions and data structures for assembling and receiving XBee packets.<br />
<br />
=====int readPacket( void)=====<br />
Parses XBee data from UART2. Makes it accessable in the ''InPacket'' struct.<br />
<br />
The UART2 receive ISR takes error codes from this function and flashes the corresponding puck LED (absolute value of the error code).<br />
<br />
Error codes are:<br />
<br />
<code><pre><br />
//////////////ERROR CODES/////////////////<br />
// <br />
// -1: Timeout waiting for UART2<br />
// -2: Start delimiter wrong<br />
// -3: Checksum Error<br />
// -4: UART2 not ready at beginning<br />
// -5: API_ID unidentified<br />
// -6: Send Packet CCA failuue<br />
// -7: Modem Status packet failure<br />
//////////////////////////////////////////<br />
</pre></code><br />
<br />
===send_packet(.c/.h)===<br />
* .c: Contains the void send_packet(void) function which fills an array with data and calls the needed XBee functions to send a packet.<br />
* .h: Contains function prototype.<br />
<br />
=====void send_packet(void)=====<br />
* Creates ''packet'' array.<br />
* Adds consensus estimator data to the array.<br />
* Adds robot statistics to the array.<br />
* Adds color sensor values to the array.<br />
* Asserts flow control line to stop XBee from sending<br />
* Calls assemblePacket to send the packet.<br />
* Deasserts flow control; Xbee can send again.<br />
<br />
<br />
The function e_get_acc_filtered returns a running average of the acc specified. Syntax is:<br />
<br />
return value = e_get_acc_filtered(acc_channel, number of samples to average)<br />
<br />
The number of samples averaged must be less than ACC_SAMP_NB as defined in e_ad_conv.h.<br />
<br />
<code><pre><br />
packet[i]=e_get_acc_filtered(2, 136); // red<br />
i++;<br />
packet[i]=e_get_acc_filtered(1, 136); // green<br />
i++;<br />
packet[i]=e_get_acc_filtered(0, 136); // blue<br />
</pre></code><br />
<br />
===PI_consensus_estimator(.h)===<br />
* .h: Contains functions and data structures for the PI consensus estimator. <br />
* This will probably be replaced by the algorithm for sensor consensus. <br />
<br />
===wheel_speed_coordinator(.c/.h)===<br />
* .c: Contains functions for robot motion control<br />
* .h: Function prototypes and variabls.<br />
<br />
=====void wheelSpeed(int *vL, int *vR)=====<br />
Return needed wheel speeds for the inertial consensus estimator based on the group goal.<br />
<br />
=====void wheelSpeedSingleBot(float gotox, float gotoy, int *vL, int *vR)=====<br />
Return needed wheel speed to get this individual bot to (gotox, gotoy). It's a hacked fix. Should be replaced with the 3 step motion controller from NUtest.c.<br />
<br />
===e_acc(.c/.h)===<br />
* .c: Functions for reading the accelerometers (which is the color sensor).<br />
* .h: Function prototypes.<br />
<br />
This is original e-puck library code with the following modifications:<br />
<br />
<code><pre><br />
//changed by Sam, July 10, default offset is 2000. we want 0 for RGB sensor.<br />
static int centre_z = 0; //zero value for z axe<br />
</pre></code><br />
<br />
=====int e_get_acc_filtered(unsigned int captor, unsigned int filter_size)=====<br />
<br />
===e_ad_conv(.c/.h)===<br />
Set up the ADCs on the puck. Original e-puck library code, with the following modifications<br />
* .h: Define constants and functional prototypes<br />
<br />
MIC_SAMP_FREQ sets the baseline sampling frequency for the ADC, everything else must be a fraction of this. 16384 Hz is the highest possible.<br />
<code><pre><br />
#define MIC_SAMP_FREQ 16384.0 <br />
</pre></code><br />
<br />
ACC_PROX_SAMP_FREQ sets the sampling frequency for the accelerometers (color sensor). We found in testing that the puck become non-responsive with this set to 8192 Hz or 16384 Hz.<br />
<code><pre><br />
// sampling frequency for the accelerometres and proximetres<br />
//#define ACC_PROX_SAMP_FREQ 256.0 // WARNING: should be a fraction of MIC_SAMP_FREQ<br />
#define ACC_PROX_SAMP_FREQ 4096 // to ensure a good timing precision<br />
// So your options are: 1 2 4 8 16 32 64 128 <br />
// 256 512 1024 2048 4096 8192 16384<br />
</pre></code><br />
<br />
ACC_SAMP_NB is the number of samples to store. We can do an average of ''up to'' this many samples. This is set to 140 so we can average 136 samples, which is 4 projector periods.<br />
<code><pre><br />
#define ACC_SAMP_NB 140 // number of accelerometer samples to store<br />
</pre></code><br />
<br />
* .c: Functions and interrupt service routines for ADCs. Original e-puck library, no modifications.<br />
<br />
=====e_init_ad_scan(ALL_ADC)=====<br />
Call to setup ADC and have it work in the background. Use e_acc functions to access data.<br />
<br />
===e_init_port(.c/.h)===<br />
* .c/.h: Initializes the ports on the e-puck. File is from the standard e-puck library. <br />
<br />
=====e_init_ports(void)=====<br />
This function sets up ports on the e-puck. Call before using any ports.<br />
<br />
===e_led(.c/.h)===<br />
* .c/.h: This is a standard e-puck library file that contains functions for manipulating LEDs.<br />
<br />
=====void e_set_led(unsigned int led_number, unsigned int value)=====<br />
Set led_number (0-7) to value (0=off 1=on higher=inverse).<br />
<br />
[[Image:e-puck_LED_numbering.png|thumb|left]]<br />
<br />
<br clear='all'><br />
<br />
===e_motors_swarm(.c/.h)===<br />
* .c/.h: This file is a modified version of the e_motors.h e-puck library file. This version keeps track of the robot's position and orientation, and the motor stepping function contains code to update the robot's position when the wheels turn. <br />
* The functions like e_rotate and e_translate have been removed and this version is not dependent on e_agenda. <br />
<br />
=====#define POINT_OFFSET -31.75 =====<br />
In addition to constants like wheel radius and wheel base, the offset between the center point of the bot and the point we're driving is set in e_motors_swarm.h. -31.75 mm is the distance betwene the center and the color sensor. So technically, we're driving backwards.<br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T5Interrupt(void)=====<br />
Timer5 ISR, interrupt for left motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos + cval*LINSTEP;<br />
ypos = ypos + sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T4Interrupt(void)=====<br />
Timer4 ISR, interrupt for right motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos - cval*LINSTEP;<br />
ypos = ypos - sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void e_init_motors(void)=====<br />
Call this function before other motor functions to initialize the motors.<br />
<br />
=====void e_set_speed_left(int motor_speed)/void e_set_speed_right(int motor_speed)=====<br />
Set the motor speed in steps/second.<br />
<br />
=====void e_get_configuration(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the center reference point.<br />
<br />
=====void e_set_configuration(float x, float y, float theta)=====<br />
Sets x, y, theta to values. This is used to overwrite the puck position/orientation estimates with data from the vision system.<br />
<br />
=====void e_get_configuration_front(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the front reference point (used for motor control).<br />
<br />
<br />
[[Category:SwarmRobotProject]]<br />
[[Category:e-puck]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=File:Swarm-robotics-flow-chart-epuck-all.pdf&diff=18228File:Swarm-robotics-flow-chart-epuck-all.pdf2010-04-09T19:28:53Z<p>Sam Bobb: </p>
<hr />
<div></div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=18187High Speed Motor Control2010-03-19T10:49:38Z<p>Sam Bobb: /* Overview */</p>
<hr />
<div>[[Image:2dofArmSetUp.jpg||right|link=Results]]<br />
<br />
==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path. These equations were found on [http://www.societyofrobots.com/robot_arm_tutorial.shtml#forward_kinematics this helpful website]. The following block of MATLAB code shows how these equations were adapted for this project.<br />
<br />
<pre><br />
function [ x, y ] = arm_thetas_to_xy( theta1, theta2 )<br />
%ARM_THETAS_TO_XY Uses forward kinematics to give position from angle<br />
% Pass in theta1 and theta2 (in degrees)<br />
% Returns x and y<br />
% ---<br />
% Sam Bobb, Daniel Cornew, Ryan Deeter<br />
% Two degree of freedom arm<br />
% High speed motor control for 2 motors<br />
% Code version 3<br />
% March 18, 2010<br />
% ---<br />
<br />
% pull in constants<br />
arm_constants<br />
<br />
alpha = deg2rad(theta1);<br />
psi = deg2rad(theta2 - theta1);<br />
<br />
j1_x = L1*cos(alpha);<br />
j1_y = L1*sin(alpha);<br />
<br />
j2_x = L2*cos(alpha+psi) + j1_x;<br />
j2_y = L2*sin(alpha+psi) + j1_y;<br />
<br />
x = j2_x;<br />
y = j2_y;<br />
<br />
end<br />
</pre><br />
<br />
<pre><br />
function [ theta1, theta2 ] = arm_xy_to_thetas( x, y )<br />
%ARM_XY_TO_THETAS Uses inversion kinematics to give angles for position<br />
% Pass in x and y<br />
% Returns theta1 and theta2 (in degrees)<br />
% ---<br />
% Sam Bobb, Daniel Cornew, Ryan Deeter<br />
% Two degree of freedom arm<br />
% High speed motor control for 2 motors<br />
% Code version 3<br />
% March 18, 2010<br />
% ---<br />
<br />
% pull in constants<br />
arm_constants<br />
<br />
% calculate the theta path <br />
c2 = (x.^2 + y.^2 - L1.^2 - L2.^2) / (2 * L1 * L2);<br />
s2 = sqrt(1 - c2.^2);<br />
psi = acos((x.^2 + y.^2 - L1^2 - L2^2) / (2 * L1 * L2));<br />
alpha = asin((y .* (L1 + L2 .* c2) - x .* L2 .* s2) ./ (x.^2 + y.^2));<br />
<br />
% convert to degrees<br />
theta1 = rad2deg(alpha);<br />
theta2 = rad2deg(alpha+psi);<br />
<br />
end<br />
</pre><br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
[[Image:2dof-interconnects.png|thumb|300px|Electrical block diagram.|right]]<br />
[[Image:2dof-hbridge.png|thumb|300px|H-bridge circuit and opto-isolators.|right]]<br />
[[Image:2dof-encoder.png|thumb|300px|Quadrature encoder decoder circuit (one for each motor)|right]]<br />
<br />
<br />
Electronics consist of decoder circuits for the encoders on each motor, an H-bridge circuit for each motor connected to a PWM output on the PIC and two digital IO pins for direction, the serial cable connection to the PC, LEDs for error alerts, and optical break sensors to measure the absolute position of the arm.<br />
<br />
The quadrature output from the motors is decoded and sent to the PIC with the LS7083 chip. A 332 kilo-ohm resistor provides an appropriate pulse output width.<br />
<br />
Each motor is driven by one L298 dual H-bridge package. This package contains two H-bridges which are connected in parallel to increase the current handling ability. It is important to connect parallel the two H-bridges within the chip in exactly the way shown in the diagram. Heat sinks are required on these chips. High speed fly-back diodes are required to protect the H-bridge chips. Schottkey diodes were used because they provide almost instant reverse recovery time. <br />
<br />
<br />
<br clear=all><br />
<br />
===Components===<br />
<br />
{| class="wikitable" border="1"<br />
|-<br />
! Electrical Components Needed.<br />
! Quantity<br />
! Data Sheets<br />
<br />
|-<br />
| PIC 32 on NU32 board<br />
| 1<br />
| [[Introduction to the PIC32|Introduction to the PIC32]]<br />
|-<br />
| H-bridges L298<br />
| 2<br />
| [http://www.st.com/stonline/books/pdf/docs/1773.pdf data sheet]<br />
|-<br />
| Optoisolators 4N27<br />
| 6<br />
| [http://media.digikey.com/pdf/Data%20Sheets/Lite-On%20PDFs/4N27_28_.pdf data sheet]<br />
|-<br />
| Quadrature Up/Down decoders LS7083<br />
| 2<br />
| [http://www.datasheetcatalog.com/datasheets_pdf/L/S/7/0/LS7083.shtml data sheet]<br />
|-<br />
| QVB11134 Optointerrupters<br />
| 2<br />
| [http://media.digikey.com/pdf/Data%20Sheets/Fairchild%20PDFs/QVB%20Series.pdf data sheet]<br />
|-<br />
| 35V 9A Schottkey Diodes 90SQ035-ND<br />
| 8<br />
| [http://www.vishay.com/doc?93417 data sheet]<br />
|-<br />
| Pittman GM8224 motor with 19.5:1 gearhead and 500 line encoder<br />
| 2<br />
| [[Media:pittmangearmotor.pdf|data sheet]]<br />
|-<br />
| 24VDC 6A power supply<br />
| 1<br />
| N/A<br />
|}<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[[Image: guiScreenShot.jpg|500px|center|Screenshot of the GUI]]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in the dark color is the path that the PIC tried to execute, and the light color plot shows the actual path of the arm, for comparison. Currently, the plotting commands are buggy. Data only plot if "Continuous Update" is on, and the "Get Data" button does not seem to function. The problem here is communicating between the GUI and the other MATLAB functions, and this could easily be corrected.<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. This table shows the available commands. <br />
{| class="wikitable" border="1"<br />
|+Commands<br />
|-<br />
! Command !! Identifying Integer !! Description !! Number of following ints !! Syntax for following ints<br />
|-<br />
! C_HOLD<br />
| 0 || Hold arm in home position || 2 || [ x_position y_position ]<br />
|-<br />
! C_FLOAT<br />
| 1 || Stop and float motors || 1 || [ true = 1 ]<br />
|-<br />
! C_RUN<br />
| 2 || Run path || 1 || [ true = 1 ]<br />
|-<br />
! C_CONTROL_CONFIG<br />
| 3 || Send new control parameters || 14 || [ KPnum1 KPden1 KDnum1 KDden1 KInum1 KIden1 Kslope1 KPnum2 KPden2 KDnum2 KDden2 KInum2 KIden2 Kslope2 ]<br />
|-<br />
! C_LOG_CONFIG<br />
| 4 || Send new logging parameters (unimplemented) || 0 || --<br />
|-<br />
! C_SET_PATH<br />
| 5 || Send new path points || variable || [ n time<sub>1</sub> theta1<sub>1</sub> theta2<sub>1</sub> time<sub>2</sub> theta1<sub>2</sub> theta2<sub>2</sub> time<sub>3</sub> theta1<sub>3</sub> theta2<sub>3</sub> ... time<sub>n</sub> theta1<sub>n</sub> theta2<sub>n</sub> ]<br />
|-<br />
! C_SEND_LOG<br />
| 6 || Asks the PIC to send logged data || 0 || -- <br />
|-<br />
! C_LOOP<br />
| 7 || Sets the looping option || 1 || 0 = run once, 1 = loop the path <br />
|-<br />
! C_STARTSTOP<br />
| 8 || start and stop (unimplemented) || 0 || -- <br />
|-<br />
! C_GOTO<br />
| 9 || Tells the arm to goto a certain X,Y || 2 || [ X Y ]<br />
|-<br />
! C_PAUSE<br />
| 10 || Stop and hold at current location || 0 || --<br />
|-<br />
|}<br />
<br />
For example, the vector that MATLAB sends to move the arm to X = 15, Y = -10 would be: [9 15 -10].<br />
<br />
===PIC C Code===<br />
[[Media:2dof-arm-v3-PIC.zip]]<br />
<br />
This code runs on the PIC 32 and handles the motor control, control loop, logging, and serial interface. On start up, the PIC runs the initialization routines, and then waits for the user to move the arm through each of the optical break sensors. When a break is detected on one of the sensors, the position of the corresponding arm is set to zero. This allows an absolute position to be established for each arm. Once the procedure is complete, the PIC waits for commands from the PC.<br />
====Initialization====<br />
* mInitAllLEDs() -- sets up the on board LEDs<br />
* initPWMandIO() -- sets up digital IO pins for motors and more LEDs, sets up two analog to digital converters for the optical break sensors, configures the PWM modules and assigns them to Timer 3, and configures Timer3 at 20kHz<br />
* initEncoder() -- Sets up Timers 1, 2, 4, and 5 as external counters for the encoders.<br />
* initUART2(pbClk) -- Sets up UART2 for serial communication.<br />
====Timed loop (2 kHz)====<br />
Every 0.5 ms the control loop runs. This loop updates the current theta values of each motor by reading the encoder counts. It then checks the limits on the arms to make sure they have not gone out of acceptable ranges. Then the new reference thetas are calculated based on the current time and the path points. The reference and current thetas are used to calculate error and then run the PID control which sets the motor outputs. Data is recorded to the log every 20 control loops.<br />
====Serial Interrupt====<br />
When the PIC receives serial data, the first two bytes are read and assembled into one 16 bit integer. This integer is then matched to the command table to identify the command. For each command, the correct number of extra integers are read from the serial buffer and global variables are filled in to adjust control parameters, path points, and runtime options.<br />
<br />
===MATLAB Code===<br />
[[Media:2dof-arm-v3-matlab.zip]]<br />
<br />
The MATLAB library consists of functions for each of the commands that can be sent to the PIC and some helper functions. When one of the callback functions from the GUI runs, it pulls parameters from the interface, assembles them, and passes them to the proper command functions.<br />
<br />
Within the arm command functions, MATLAB uses the ''fwrite'' function to write data to the serial port. The ''int16'' option ensures that MATLAB breaks each number into a 16 bit integer and sends it as two bytes out the serial port. The PIC will then reassemble these two bytes as a 16 bit integer.<br />
<br />
====Example MATLAB procedure====<br />
Suppose the user wants move the arm to X = 15, Y = -10. The following happens:<br />
# The user opens the GUI by running me333gui from the MATLAB command line<br />
# The user puts the proper COM port in the COM port text box and clicks Connect<br />
# The function ''Connect_Callback'' runs. This function pulls the text from COMport text box and sends it to the ''arm_connect'' function. The serial port is now open. The handle for the serial port object is stored in the UserData field.<br />
# The user puts the desired X and Y in the goto X and Y text boxes, selects the GoTo radio button, and clicks Start.<br />
# The function ''startButton_Callback''. This function checks which of the mode radio buttons are selected. If GoTo is selected, it reads from the X and Y boxes, and calls the ''arm_goto'' function with the COM port handle and the X and Y from the text box.<br />
# The arm receives the command and executes the move.<br />
<br />
==Results==<br />
[http://www.youtube.com/watch?v=Ffun2e6hnVE This video] demonstration shows the capabilities of the arm.<br />
<br />
<br />
Before paths are executed, the arm is lifted manually so that the optointerrupters are tripped and the arm is "homed," meaning that the PIC recognizes the arm's location with respect to motor encoder counts.<br />
<br />
The first path executed is a simple "Go To." <br />
<br />
[[Image: TraceTheta.jpg|thumb|150px|right]]<br />
After the "Go To," a looped "Trace" path is executed. The GUI screenshot at the right was used to generate this path. The top screenshot shows theta plotted against time and the bottom shows position plotted in xy space.<br />
[[Image: TraceXY.jpg|thumb|150px|right]]<br />
<br />
<br clear=all><br />
<br />
[[Image: CircleGUI.jpg|thumb|150px|right]]<br />
The last two paths are both circles. The first is a slow, wide circle rotating counter clockwise. The second is a quicker, tighter circle rotating clockwise. The GUI for the wide circle is at the right.<br />
<br />
<br clear=all><br />
<br />
==Next Steps==<br />
This project came a long way, but could go much further. <br />
<br />
One large area of improvement is in the logging of data. Logging speeds could be improved through more efficient coding, but even more through the use of USB communication. Since data are currently sent through RS-232, top transmission speeds are limited. Using USB would allow for the GUI graphs to update in much closer to real time. <br />
<br />
The motor control parameters could be tweaked to provide for greater control. Part of that is just trail and error for different constants, but better derivative and integral terms could be used in the control.<br />
<br />
Mechanical improvements would also aid in control. The arms currently move about and bounce (as seen in the video) due to motor backlash. Fixing this problem would allow for even more precise, quicker motions.<br />
<br />
The GUI could also be greatly improved to allow for more user options. The options were limited in this version to reduce the likelihood that a user would program a path fatal to the arm. With improved software and hardware limit switching, more user control would be available.<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=18186High Speed Motor Control2010-03-19T10:49:18Z<p>Sam Bobb: /* Electrical Design */</p>
<hr />
<div>[[Image:2dofArmSetUp.jpg||right|link=Results]]<br />
<br />
==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path. These equations were found on [http://www.societyofrobots.com/robot_arm_tutorial.shtml#forward_kinematics this helpful website]. The following block of MATLAB code shows how these equations were adapted for this project.<br />
<br />
<pre><br />
function [ x, y ] = arm_thetas_to_xy( theta1, theta2 )<br />
%ARM_THETAS_TO_XY Uses forward kinematics to give position from angle<br />
% Pass in theta1 and theta2 (in degrees)<br />
% Returns x and y<br />
% ---<br />
% Sam Bobb, Daniel Cornew, Ryan Deeter<br />
% Two degree of freedom arm<br />
% High speed motor control for 2 motors<br />
% Code version 3<br />
% March 18, 2010<br />
% ---<br />
<br />
% pull in constants<br />
arm_constants<br />
<br />
alpha = deg2rad(theta1);<br />
psi = deg2rad(theta2 - theta1);<br />
<br />
j1_x = L1*cos(alpha);<br />
j1_y = L1*sin(alpha);<br />
<br />
j2_x = L2*cos(alpha+psi) + j1_x;<br />
j2_y = L2*sin(alpha+psi) + j1_y;<br />
<br />
x = j2_x;<br />
y = j2_y;<br />
<br />
end<br />
</pre><br />
<br />
<pre><br />
function [ theta1, theta2 ] = arm_xy_to_thetas( x, y )<br />
%ARM_XY_TO_THETAS Uses inversion kinematics to give angles for position<br />
% Pass in x and y<br />
% Returns theta1 and theta2 (in degrees)<br />
% ---<br />
% Sam Bobb, Daniel Cornew, Ryan Deeter<br />
% Two degree of freedom arm<br />
% High speed motor control for 2 motors<br />
% Code version 3<br />
% March 18, 2010<br />
% ---<br />
<br />
% pull in constants<br />
arm_constants<br />
<br />
% calculate the theta path <br />
c2 = (x.^2 + y.^2 - L1.^2 - L2.^2) / (2 * L1 * L2);<br />
s2 = sqrt(1 - c2.^2);<br />
psi = acos((x.^2 + y.^2 - L1^2 - L2^2) / (2 * L1 * L2));<br />
alpha = asin((y .* (L1 + L2 .* c2) - x .* L2 .* s2) ./ (x.^2 + y.^2));<br />
<br />
% convert to degrees<br />
theta1 = rad2deg(alpha);<br />
theta2 = rad2deg(alpha+psi);<br />
<br />
end<br />
</pre><br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
[[Image:2dof-interconnects.png|thumb|300px|Electrical block diagram.|right]]<br />
[[Image:2dof-hbridge.png|thumb|300px|H-bridge circuit and opto-isolators.|right]]<br />
[[Image:2dof-encoder.png|thumb|300px|Quadrature encoder decoder circuit (one for each motor)|right]]<br />
<br />
<br />
Electronics consist of decoder circuits for the encoders on each motor, an H-bridge circuit for each motor connected to a PWM output on the PIC and two digital IO pins for direction, the serial cable connection to the PC, LEDs for error alerts, and optical break sensors to measure the absolute position of the arm.<br />
<br />
The quadrature output from the motors is decoded and sent to the PIC with the LS7083 chip. A 332 kilo-ohm resistor provides an appropriate pulse output width.<br />
<br />
Each motor is driven by one L298 dual H-bridge package. This package contains two H-bridges which are connected in parallel to increase the current handling ability. It is important to connect parallel the two H-bridges within the chip in exactly the way shown in the diagram. Heat sinks are required on these chips. High speed fly-back diodes are required to protect the H-bridge chips. Schottkey diodes were used because they provide almost instant reverse recovery time. <br />
<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
<br />
<br clear=all><br />
<br />
===Components===<br />
<br />
{| class="wikitable" border="1"<br />
|-<br />
! Electrical Components Needed.<br />
! Quantity<br />
! Data Sheets<br />
<br />
|-<br />
| PIC 32 on NU32 board<br />
| 1<br />
| [[Introduction to the PIC32|Introduction to the PIC32]]<br />
|-<br />
| H-bridges L298<br />
| 2<br />
| [http://www.st.com/stonline/books/pdf/docs/1773.pdf data sheet]<br />
|-<br />
| Optoisolators 4N27<br />
| 6<br />
| [http://media.digikey.com/pdf/Data%20Sheets/Lite-On%20PDFs/4N27_28_.pdf data sheet]<br />
|-<br />
| Quadrature Up/Down decoders LS7083<br />
| 2<br />
| [http://www.datasheetcatalog.com/datasheets_pdf/L/S/7/0/LS7083.shtml data sheet]<br />
|-<br />
| QVB11134 Optointerrupters<br />
| 2<br />
| [http://media.digikey.com/pdf/Data%20Sheets/Fairchild%20PDFs/QVB%20Series.pdf data sheet]<br />
|-<br />
| 35V 9A Schottkey Diodes 90SQ035-ND<br />
| 8<br />
| [http://www.vishay.com/doc?93417 data sheet]<br />
|-<br />
| Pittman GM8224 motor with 19.5:1 gearhead and 500 line encoder<br />
| 2<br />
| [[Media:pittmangearmotor.pdf|data sheet]]<br />
|-<br />
| 24VDC 6A power supply<br />
| 1<br />
| N/A<br />
|}<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[[Image: guiScreenShot.jpg|500px|center|Screenshot of the GUI]]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in the dark color is the path that the PIC tried to execute, and the light color plot shows the actual path of the arm, for comparison. Currently, the plotting commands are buggy. Data only plot if "Continuous Update" is on, and the "Get Data" button does not seem to function. The problem here is communicating between the GUI and the other MATLAB functions, and this could easily be corrected.<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. This table shows the available commands. <br />
{| class="wikitable" border="1"<br />
|+Commands<br />
|-<br />
! Command !! Identifying Integer !! Description !! Number of following ints !! Syntax for following ints<br />
|-<br />
! C_HOLD<br />
| 0 || Hold arm in home position || 2 || [ x_position y_position ]<br />
|-<br />
! C_FLOAT<br />
| 1 || Stop and float motors || 1 || [ true = 1 ]<br />
|-<br />
! C_RUN<br />
| 2 || Run path || 1 || [ true = 1 ]<br />
|-<br />
! C_CONTROL_CONFIG<br />
| 3 || Send new control parameters || 14 || [ KPnum1 KPden1 KDnum1 KDden1 KInum1 KIden1 Kslope1 KPnum2 KPden2 KDnum2 KDden2 KInum2 KIden2 Kslope2 ]<br />
|-<br />
! C_LOG_CONFIG<br />
| 4 || Send new logging parameters (unimplemented) || 0 || --<br />
|-<br />
! C_SET_PATH<br />
| 5 || Send new path points || variable || [ n time<sub>1</sub> theta1<sub>1</sub> theta2<sub>1</sub> time<sub>2</sub> theta1<sub>2</sub> theta2<sub>2</sub> time<sub>3</sub> theta1<sub>3</sub> theta2<sub>3</sub> ... time<sub>n</sub> theta1<sub>n</sub> theta2<sub>n</sub> ]<br />
|-<br />
! C_SEND_LOG<br />
| 6 || Asks the PIC to send logged data || 0 || -- <br />
|-<br />
! C_LOOP<br />
| 7 || Sets the looping option || 1 || 0 = run once, 1 = loop the path <br />
|-<br />
! C_STARTSTOP<br />
| 8 || start and stop (unimplemented) || 0 || -- <br />
|-<br />
! C_GOTO<br />
| 9 || Tells the arm to goto a certain X,Y || 2 || [ X Y ]<br />
|-<br />
! C_PAUSE<br />
| 10 || Stop and hold at current location || 0 || --<br />
|-<br />
|}<br />
<br />
For example, the vector that MATLAB sends to move the arm to X = 15, Y = -10 would be: [9 15 -10].<br />
<br />
===PIC C Code===<br />
[[Media:2dof-arm-v3-PIC.zip]]<br />
<br />
This code runs on the PIC 32 and handles the motor control, control loop, logging, and serial interface. On start up, the PIC runs the initialization routines, and then waits for the user to move the arm through each of the optical break sensors. When a break is detected on one of the sensors, the position of the corresponding arm is set to zero. This allows an absolute position to be established for each arm. Once the procedure is complete, the PIC waits for commands from the PC.<br />
====Initialization====<br />
* mInitAllLEDs() -- sets up the on board LEDs<br />
* initPWMandIO() -- sets up digital IO pins for motors and more LEDs, sets up two analog to digital converters for the optical break sensors, configures the PWM modules and assigns them to Timer 3, and configures Timer3 at 20kHz<br />
* initEncoder() -- Sets up Timers 1, 2, 4, and 5 as external counters for the encoders.<br />
* initUART2(pbClk) -- Sets up UART2 for serial communication.<br />
====Timed loop (2 kHz)====<br />
Every 0.5 ms the control loop runs. This loop updates the current theta values of each motor by reading the encoder counts. It then checks the limits on the arms to make sure they have not gone out of acceptable ranges. Then the new reference thetas are calculated based on the current time and the path points. The reference and current thetas are used to calculate error and then run the PID control which sets the motor outputs. Data is recorded to the log every 20 control loops.<br />
====Serial Interrupt====<br />
When the PIC receives serial data, the first two bytes are read and assembled into one 16 bit integer. This integer is then matched to the command table to identify the command. For each command, the correct number of extra integers are read from the serial buffer and global variables are filled in to adjust control parameters, path points, and runtime options.<br />
<br />
===MATLAB Code===<br />
[[Media:2dof-arm-v3-matlab.zip]]<br />
<br />
The MATLAB library consists of functions for each of the commands that can be sent to the PIC and some helper functions. When one of the callback functions from the GUI runs, it pulls parameters from the interface, assembles them, and passes them to the proper command functions.<br />
<br />
Within the arm command functions, MATLAB uses the ''fwrite'' function to write data to the serial port. The ''int16'' option ensures that MATLAB breaks each number into a 16 bit integer and sends it as two bytes out the serial port. The PIC will then reassemble these two bytes as a 16 bit integer.<br />
<br />
====Example MATLAB procedure====<br />
Suppose the user wants move the arm to X = 15, Y = -10. The following happens:<br />
# The user opens the GUI by running me333gui from the MATLAB command line<br />
# The user puts the proper COM port in the COM port text box and clicks Connect<br />
# The function ''Connect_Callback'' runs. This function pulls the text from COMport text box and sends it to the ''arm_connect'' function. The serial port is now open. The handle for the serial port object is stored in the UserData field.<br />
# The user puts the desired X and Y in the goto X and Y text boxes, selects the GoTo radio button, and clicks Start.<br />
# The function ''startButton_Callback''. This function checks which of the mode radio buttons are selected. If GoTo is selected, it reads from the X and Y boxes, and calls the ''arm_goto'' function with the COM port handle and the X and Y from the text box.<br />
# The arm receives the command and executes the move.<br />
<br />
==Results==<br />
[http://www.youtube.com/watch?v=Ffun2e6hnVE This video] demonstration shows the capabilities of the arm.<br />
<br />
<br />
Before paths are executed, the arm is lifted manually so that the optointerrupters are tripped and the arm is "homed," meaning that the PIC recognizes the arm's location with respect to motor encoder counts.<br />
<br />
The first path executed is a simple "Go To." <br />
<br />
[[Image: TraceTheta.jpg|thumb|150px|right]]<br />
After the "Go To," a looped "Trace" path is executed. The GUI screenshot at the right was used to generate this path. The top screenshot shows theta plotted against time and the bottom shows position plotted in xy space.<br />
[[Image: TraceXY.jpg|thumb|150px|right]]<br />
<br />
<br clear=all><br />
<br />
[[Image: CircleGUI.jpg|thumb|150px|right]]<br />
The last two paths are both circles. The first is a slow, wide circle rotating counter clockwise. The second is a quicker, tighter circle rotating clockwise. The GUI for the wide circle is at the right.<br />
<br />
<br clear=all><br />
<br />
==Next Steps==<br />
This project came a long way, but could go much further. <br />
<br />
One large area of improvement is in the logging of data. Logging speeds could be improved through more efficient coding, but even more through the use of USB communication. Since data are currently sent through RS-232, top transmission speeds are limited. Using USB would allow for the GUI graphs to update in much closer to real time. <br />
<br />
The motor control parameters could be tweaked to provide for greater control. Part of that is just trail and error for different constants, but better derivative and integral terms could be used in the control.<br />
<br />
Mechanical improvements would also aid in control. The arms currently move about and bounce (as seen in the video) due to motor backlash. Fixing this problem would allow for even more precise, quicker motions.<br />
<br />
The GUI could also be greatly improved to allow for more user options. The options were limited in this version to reduce the likelihood that a user would program a path fatal to the arm. With improved software and hardware limit switching, more user control would be available.<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=18185High Speed Motor Control2010-03-19T10:41:02Z<p>Sam Bobb: /* Electrical Design */</p>
<hr />
<div>[[Image:2dofArmSetUp.jpg||right|link=Results]]<br />
<br />
==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path. These equations were found on [http://www.societyofrobots.com/robot_arm_tutorial.shtml#forward_kinematics this helpful website]. The following block of MATLAB code shows how these equations were adapted for this project.<br />
<br />
<pre><br />
function [ x, y ] = arm_thetas_to_xy( theta1, theta2 )<br />
%ARM_THETAS_TO_XY Uses forward kinematics to give position from angle<br />
% Pass in theta1 and theta2 (in degrees)<br />
% Returns x and y<br />
% ---<br />
% Sam Bobb, Daniel Cornew, Ryan Deeter<br />
% Two degree of freedom arm<br />
% High speed motor control for 2 motors<br />
% Code version 3<br />
% March 18, 2010<br />
% ---<br />
<br />
% pull in constants<br />
arm_constants<br />
<br />
alpha = deg2rad(theta1);<br />
psi = deg2rad(theta2 - theta1);<br />
<br />
j1_x = L1*cos(alpha);<br />
j1_y = L1*sin(alpha);<br />
<br />
j2_x = L2*cos(alpha+psi) + j1_x;<br />
j2_y = L2*sin(alpha+psi) + j1_y;<br />
<br />
x = j2_x;<br />
y = j2_y;<br />
<br />
end<br />
</pre><br />
<br />
<pre><br />
function [ theta1, theta2 ] = arm_xy_to_thetas( x, y )<br />
%ARM_XY_TO_THETAS Uses inversion kinematics to give angles for position<br />
% Pass in x and y<br />
% Returns theta1 and theta2 (in degrees)<br />
% ---<br />
% Sam Bobb, Daniel Cornew, Ryan Deeter<br />
% Two degree of freedom arm<br />
% High speed motor control for 2 motors<br />
% Code version 3<br />
% March 18, 2010<br />
% ---<br />
<br />
% pull in constants<br />
arm_constants<br />
<br />
% calculate the theta path <br />
c2 = (x.^2 + y.^2 - L1.^2 - L2.^2) / (2 * L1 * L2);<br />
s2 = sqrt(1 - c2.^2);<br />
psi = acos((x.^2 + y.^2 - L1^2 - L2^2) / (2 * L1 * L2));<br />
alpha = asin((y .* (L1 + L2 .* c2) - x .* L2 .* s2) ./ (x.^2 + y.^2));<br />
<br />
% convert to degrees<br />
theta1 = rad2deg(alpha);<br />
theta2 = rad2deg(alpha+psi);<br />
<br />
end<br />
</pre><br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
[[Image:2dof-interconnects.png|thumb|300px|Electrical block diagram.|right]]<br />
[[Image:2dof-hbridge.png|thumb|300px|H-bridge circuit and opto-isolators.|right]]<br />
[[Image:2dof-encoder.png|thumb|300px|Quadrature encoder decoder circuit (one for each motor)|right]]<br />
<br />
<br />
<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
<br />
<br clear=all><br />
<br />
===Components===<br />
<br />
{| class="wikitable" border="1"<br />
|-<br />
! Electrical Components Needed.<br />
! Quantity<br />
! Data Sheets<br />
<br />
|-<br />
| PIC 32 on NU32 board<br />
| 1<br />
| [[Introduction to the PIC32|Introduction to the PIC32]]<br />
|-<br />
| H-bridges L298<br />
| 2<br />
| [http://www.st.com/stonline/books/pdf/docs/1773.pdf data sheet]<br />
|-<br />
| Optoisolators 4N27<br />
| 6<br />
| [http://media.digikey.com/pdf/Data%20Sheets/Lite-On%20PDFs/4N27_28_.pdf data sheet]<br />
|-<br />
| Quadrature Up/Down decoders LS7083<br />
| 2<br />
| [http://www.datasheetcatalog.com/datasheets_pdf/L/S/7/0/LS7083.shtml data sheet]<br />
|-<br />
| QVB11134 Optointerrupters<br />
| 2<br />
| [http://media.digikey.com/pdf/Data%20Sheets/Fairchild%20PDFs/QVB%20Series.pdf data sheet]<br />
|-<br />
| 35V 9A Schottkey Diodes 90SQ035-ND<br />
| 8<br />
| [http://www.vishay.com/doc?93417 data sheet]<br />
|-<br />
| Pittman GM8224 motor with 19.5:1 gearhead and 500 line encoder<br />
| 2<br />
| [[Media:pittmangearmotor.pdf|data sheet]]<br />
|-<br />
| 24VDC 6A power supply<br />
| 1<br />
| N/A<br />
|}<br />
<br />
===Circuit Diagram===<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[[Image: guiScreenShot.jpg|500px|center|Screenshot of the GUI]]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in the dark color is the path that the PIC tried to execute, and the light color plot shows the actual path of the arm, for comparison. Currently, the plotting commands are buggy. Data only plot if "Continuous Update" is on, and the "Get Data" button does not seem to function. The problem here is communicating between the GUI and the other MATLAB functions, and this could easily be corrected.<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. This table shows the available commands. <br />
{| class="wikitable" border="1"<br />
|+Commands<br />
|-<br />
! Command !! Identifying Integer !! Description !! Number of following ints !! Syntax for following ints<br />
|-<br />
! C_HOLD<br />
| 0 || Hold arm in home position || 2 || [ x_position y_position ]<br />
|-<br />
! C_FLOAT<br />
| 1 || Stop and float motors || 1 || [ true = 1 ]<br />
|-<br />
! C_RUN<br />
| 2 || Run path || 1 || [ true = 1 ]<br />
|-<br />
! C_CONTROL_CONFIG<br />
| 3 || Send new control parameters || 14 || [ KPnum1 KPden1 KDnum1 KDden1 KInum1 KIden1 Kslope1 KPnum2 KPden2 KDnum2 KDden2 KInum2 KIden2 Kslope2 ]<br />
|-<br />
! C_LOG_CONFIG<br />
| 4 || Send new logging parameters (unimplemented) || 0 || --<br />
|-<br />
! C_SET_PATH<br />
| 5 || Send new path points || variable || [ n time<sub>1</sub> theta1<sub>1</sub> theta2<sub>1</sub> time<sub>2</sub> theta1<sub>2</sub> theta2<sub>2</sub> time<sub>3</sub> theta1<sub>3</sub> theta2<sub>3</sub> ... time<sub>n</sub> theta1<sub>n</sub> theta2<sub>n</sub> ]<br />
|-<br />
! C_SEND_LOG<br />
| 6 || Asks the PIC to send logged data || 0 || -- <br />
|-<br />
! C_LOOP<br />
| 7 || Sets the looping option || 1 || 0 = run once, 1 = loop the path <br />
|-<br />
! C_STARTSTOP<br />
| 8 || start and stop (unimplemented) || 0 || -- <br />
|-<br />
! C_GOTO<br />
| 9 || Tells the arm to goto a certain X,Y || 2 || [ X Y ]<br />
|-<br />
! C_PAUSE<br />
| 10 || Stop and hold at current location || 0 || --<br />
|-<br />
|}<br />
<br />
For example, the vector that MATLAB sends to move the arm to X = 15, Y = -10 would be: [9 15 -10].<br />
<br />
===PIC C Code===<br />
[[Media:2dof-arm-v3-PIC.zip]]<br />
<br />
This code runs on the PIC 32 and handles the motor control, control loop, logging, and serial interface. On start up, the PIC runs the initialization routines, and then waits for the user to move the arm through each of the optical break sensors. When a break is detected on one of the sensors, the position of the corresponding arm is set to zero. This allows an absolute position to be established for each arm. Once the procedure is complete, the PIC waits for commands from the PC.<br />
====Initialization====<br />
* mInitAllLEDs() -- sets up the on board LEDs<br />
* initPWMandIO() -- sets up digital IO pins for motors and more LEDs, sets up two analog to digital converters for the optical break sensors, configures the PWM modules and assigns them to Timer 3, and configures Timer3 at 20kHz<br />
* initEncoder() -- Sets up Timers 1, 2, 4, and 5 as external counters for the encoders.<br />
* initUART2(pbClk) -- Sets up UART2 for serial communication.<br />
====Timed loop (2 kHz)====<br />
Every 0.5 ms the control loop runs. This loop updates the current theta values of each motor by reading the encoder counts. It then checks the limits on the arms to make sure they have not gone out of acceptable ranges. Then the new reference thetas are calculated based on the current time and the path points. The reference and current thetas are used to calculate error and then run the PID control which sets the motor outputs. Data is recorded to the log every 20 control loops.<br />
====Serial Interrupt====<br />
When the PIC receives serial data, the first two bytes are read and assembled into one 16 bit integer. This integer is then matched to the command table to identify the command. For each command, the correct number of extra integers are read from the serial buffer and global variables are filled in to adjust control parameters, path points, and runtime options.<br />
<br />
===MATLAB Code===<br />
[[Media:2dof-arm-v3-matlab.zip]]<br />
<br />
The MATLAB library consists of functions for each of the commands that can be sent to the PIC and some helper functions. When one of the callback functions from the GUI runs, it pulls parameters from the interface, assembles them, and passes them to the proper command functions.<br />
<br />
Within the arm command functions, MATLAB uses the ''fwrite'' function to write data to the serial port. The ''int16'' option ensures that MATLAB breaks each number into a 16 bit integer and sends it as two bytes out the serial port. The PIC will then reassemble these two bytes as a 16 bit integer.<br />
<br />
====Example MATLAB procedure====<br />
Suppose the user wants move the arm to X = 15, Y = -10. The following happens:<br />
# The user opens the GUI by running me333gui from the MATLAB command line<br />
# The user puts the proper COM port in the COM port text box and clicks Connect<br />
# The function ''Connect_Callback'' runs. This function pulls the text from COMport text box and sends it to the ''arm_connect'' function. The serial port is now open. The handle for the serial port object is stored in the UserData field.<br />
# The user puts the desired X and Y in the goto X and Y text boxes, selects the GoTo radio button, and clicks Start.<br />
# The function ''startButton_Callback''. This function checks which of the mode radio buttons are selected. If GoTo is selected, it reads from the X and Y boxes, and calls the ''arm_goto'' function with the COM port handle and the X and Y from the text box.<br />
# The arm receives the command and executes the move.<br />
<br />
==Results==<br />
[http://www.youtube.com/watch?v=Ffun2e6hnVE This video] demonstration shows the capabilities of the arm.<br />
<br />
<br />
Before paths are executed, the arm is lifted manually so that the optointerrupters are tripped and the arm is "homed," meaning that the PIC recognizes the arm's location with respect to motor encoder counts.<br />
<br />
The first path executed is a simple "Go To." <br />
<br />
[[Image: TraceTheta.jpg|thumb|150px|right]]<br />
After the "Go To," a looped "Trace" path is executed. The GUI screenshot at the right was used to generate this path. The top screenshot shows theta plotted against time and the bottom shows position plotted in xy space.<br />
[[Image: TraceXY.jpg|thumb|150px|right]]<br />
<br />
<br clear=all><br />
<br />
[[Image: CircleGUI.jpg|thumb|150px|right]]<br />
The last two paths are both circles. The first is a slow, wide circle rotating counter clockwise. The second is a quicker, tighter circle rotating clockwise. The GUI for the wide circle is at the right.<br />
<br />
<br clear=all><br />
<br />
==Next Steps==<br />
This project came a long way, but could go much further. <br />
<br />
One large area of improvement is in the logging of data. Logging speeds could be improved through more efficient coding, but even more through the use of USB communication. Since data are currently sent through RS-232, top transmission speeds are limited. Using USB would allow for the GUI graphs to update in much closer to real time. <br />
<br />
The motor control parameters could be tweaked to provide for greater control. Part of that is just trail and error for different constants, but better derivative and integral terms could be used in the control.<br />
<br />
Mechanical improvements would also aid in control. The arms currently move about and bounce (as seen in the video) due to motor backlash. Fixing this problem would allow for even more precise, quicker motions.<br />
<br />
The GUI could also be greatly improved to allow for more user options. The options were limited in this version to reduce the likelihood that a user would program a path fatal to the arm. With improved software and hardware limit switching, more user control would be available.<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=File:2dof-encoder.png&diff=18184File:2dof-encoder.png2010-03-19T10:40:14Z<p>Sam Bobb: </p>
<hr />
<div></div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=18175High Speed Motor Control2010-03-19T10:09:11Z<p>Sam Bobb: /* PIC C Code */</p>
<hr />
<div>[[Image:2dofArmSetUp.jpg||right|link=Results]]<br />
<br />
==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path. These equations were found on [http://www.societyofrobots.com/robot_arm_tutorial.shtml#forward_kinematics this helpful website]. The following block of MATLAB code shows how these equations were adapted for this project.<br />
<br />
<pre><br />
function [ x, y ] = arm_thetas_to_xy( theta1, theta2 )<br />
%ARM_THETAS_TO_XY Uses forward kinematics to give position from angle<br />
% Pass in theta1 and theta2 (in degrees)<br />
% Returns x and y<br />
% ---<br />
% Sam Bobb, Daniel Cornew, Ryan Deeter<br />
% Two degree of freedom arm<br />
% High speed motor control for 2 motors<br />
% Code version 3<br />
% March 18, 2010<br />
% ---<br />
<br />
% pull in constants<br />
arm_constants<br />
<br />
alpha = deg2rad(theta1);<br />
psi = deg2rad(theta2 - theta1);<br />
<br />
j1_x = L1*cos(alpha);<br />
j1_y = L1*sin(alpha);<br />
<br />
j2_x = L2*cos(alpha+psi) + j1_x;<br />
j2_y = L2*sin(alpha+psi) + j1_y;<br />
<br />
x = j2_x;<br />
y = j2_y;<br />
<br />
end<br />
</pre><br />
<br />
<pre><br />
function [ theta1, theta2 ] = arm_xy_to_thetas( x, y )<br />
%ARM_XY_TO_THETAS Uses inversion kinematics to give angles for position<br />
% Pass in x and y<br />
% Returns theta1 and theta2 (in degrees)<br />
% ---<br />
% Sam Bobb, Daniel Cornew, Ryan Deeter<br />
% Two degree of freedom arm<br />
% High speed motor control for 2 motors<br />
% Code version 3<br />
% March 18, 2010<br />
% ---<br />
<br />
% pull in constants<br />
arm_constants<br />
<br />
% calculate the theta path <br />
c2 = (x.^2 + y.^2 - L1.^2 - L2.^2) / (2 * L1 * L2);<br />
s2 = sqrt(1 - c2.^2);<br />
psi = acos((x.^2 + y.^2 - L1^2 - L2^2) / (2 * L1 * L2));<br />
alpha = asin((y .* (L1 + L2 .* c2) - x .* L2 .* s2) ./ (x.^2 + y.^2));<br />
<br />
% convert to degrees<br />
theta1 = rad2deg(alpha);<br />
theta2 = rad2deg(alpha+psi);<br />
<br />
end<br />
</pre><br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
[[Image:2dof-interconnects.png|thumb|300px|Electrical block diagram.|right]]<br />
[[Image:2dof-hbridge.png|thumb|300px|H-bridge circuit and opto-isolators.|right]]<br />
<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
<br />
<br clear=all><br />
<br />
===Components===<br />
<br />
{| class="wikitable" border="1"<br />
|-<br />
! Electrical Components Needed.<br />
! Quantity<br />
! Data Sheets<br />
<br />
|-<br />
| PIC 32 on NU32 board<br />
| 1<br />
| [[Introduction to the PIC32|Introduction to the PIC32]]<br />
|-<br />
| H-bridges L298<br />
| 2<br />
| [http://www.st.com/stonline/books/pdf/docs/1773.pdf data sheet]<br />
|-<br />
| Optoisolators 4N27<br />
| 6<br />
| [http://media.digikey.com/pdf/Data%20Sheets/Lite-On%20PDFs/4N27_28_.pdf data sheet]<br />
|-<br />
| Quadrature Up/Down decoders LS7083<br />
| 2<br />
| [http://www.datasheetcatalog.com/datasheets_pdf/L/S/7/0/LS7083.shtml data sheet]<br />
|-<br />
| QVB11134 Optointerrupters<br />
| 2<br />
| [http://media.digikey.com/pdf/Data%20Sheets/Fairchild%20PDFs/QVB%20Series.pdf data sheet]<br />
|-<br />
| 35V 9A Schottkey Diodes 90SQ035-ND<br />
| 8<br />
| [http://www.vishay.com/doc?93417 data sheet]<br />
|-<br />
| Pittman GM8224 motor with 19.5:1 gearhead and 500 line encoder<br />
| 2<br />
| [[Media:pittmangearmotor.pdf|data sheet]]<br />
|-<br />
| 24VDC 6A power supply<br />
| 1<br />
| N/A<br />
|}<br />
<br />
===Circuit Diagram===<br />
<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[[Image: guiScreenShot.jpg|500px|center|Screenshot of the GUI]]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in the dark color is the path that the PIC tried to execute, and the light color plot shows the actual path of the arm, for comparison. Currently, the plotting commands are buggy. Data only plot if "Continuous Update" is on, and the "Get Data" button does not seem to function. The problem here is communicating between the GUI and the other MATLAB functions, and this could easily be corrected.<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. This table shows the available commands. <br />
{| class="wikitable" border="1"<br />
|+Commands<br />
|-<br />
! Command !! Identifying Integer !! Description !! Number of following ints !! Syntax for following ints<br />
|-<br />
! C_HOLD<br />
| 0 || Hold arm in home position || 2 || [ x_position y_position ]<br />
|-<br />
! C_FLOAT<br />
| 1 || Stop and float motors || 1 || [ true = 1 ]<br />
|-<br />
! C_RUN<br />
| 2 || Run path || 1 || [ true = 1 ]<br />
|-<br />
! C_CONTROL_CONFIG<br />
| 3 || Send new control parameters || 14 || [ KPnum1 KPden1 KDnum1 KDden1 KInum1 KIden1 Kslope1 KPnum2 KPden2 KDnum2 KDden2 KInum2 KIden2 Kslope2 ]<br />
|-<br />
! C_LOG_CONFIG<br />
| 4 || Send new logging parameters (unimplemented) || 0 || --<br />
|-<br />
! C_SET_PATH<br />
| 5 || Send new path points || variable || [ n time<sub>1</sub> theta1<sub>1</sub> theta2<sub>1</sub> time<sub>2</sub> theta1<sub>2</sub> theta2<sub>2</sub> time<sub>3</sub> theta1<sub>3</sub> theta2<sub>3</sub> ... time<sub>n</sub> theta1<sub>n</sub> theta2<sub>n</sub> ]<br />
|-<br />
! C_SEND_LOG<br />
| 6 || Asks the PIC to send logged data || 0 || -- <br />
|-<br />
! C_LOOP<br />
| 7 || Sets the looping option || 1 || 0 = run once, 1 = loop the path <br />
|-<br />
! C_STARTSTOP<br />
| 8 || start and stop (unimplemented) || 0 || -- <br />
|-<br />
! C_GOTO<br />
| 9 || Tells the arm to goto a certain X,Y || 2 || [ X Y ]<br />
|-<br />
! C_PAUSE<br />
| 10 || Stop and hold at current location || 0 || --<br />
|-<br />
|}<br />
<br />
For example, the vector that MATLAB sends to move the arm to X = 15, Y = -10 would be: [9 15 -10].<br />
<br />
===PIC C Code===<br />
[[Media:2dof-arm-v3-PIC.zip]]<br />
<br />
This code runs on the PIC 32 and handles the motor control, control loop, logging, and serial interface. On start up, the PIC runs the initialization routines, and then waits for the user to move the arm through each of the optical break sensors. When a break is detected on one of the sensors, the position of the corresponding arm is set to zero. This allows an absolute position to be established for each arm. Once the procedure is complete, the PIC waits for commands from the PC.<br />
====Initialization====<br />
* mInitAllLEDs() -- sets up the on board LEDs<br />
* initPWMandIO() -- sets up digital IO pins for motors and more LEDs, sets up two analog to digital converters for the optical break sensors, configures the PWM modules and assigns them to Timer 3, and configures Timer3 at 20kHz<br />
* initEncoder() -- Sets up Timers 1, 2, 4, and 5 as external counters for the encoders.<br />
* initUART2(pbClk) -- Sets up UART2 for serial communication.<br />
====Timed loop (2 kHz)====<br />
Every 0.5 ms the control loop runs. This loop updates the current theta values of each motor by reading the encoder counts. It then checks the limits on the arms to make sure they have not gone out of acceptable ranges. Then the new reference thetas are calculated based on the current time and the path points. The reference and current thetas are used to calculate error and then run the PID control which sets the motor outputs. Data is recorded to the log every 20 control loops.<br />
====Serial Interrupt====<br />
When the PIC receives serial data, the first two bytes are read and assembled into one 16 bit integer. This integer is then matched to the command table to identify the command. For each command, the correct number of extra integers are read from the serial buffer and global variables are filled in to adjust control parameters, path points, and runtime options.<br />
<br />
===MATLAB Code===<br />
[[Media:2dof-arm-v3-matlab.zip]]<br />
<br />
The MATLAB library consists of functions for each of the commands that can be sent to the PIC and some helper functions. When one of the callback functions from the GUI runs, it pulls parameters from the interface, assembles them, and passes them to the proper command functions.<br />
<br />
Within the arm command functions, MATLAB uses the ''fwrite'' function to write data to the serial port. The ''int16'' option ensures that MATLAB breaks each number into a 16 bit integer and sends it as two bytes out the serial port. The PIC will then reassemble these two bytes as a 16 bit integer.<br />
<br />
====Example MATLAB procedure====<br />
Suppose the user wants move the arm to X = 15, Y = -10. The following happens:<br />
# The user opens the GUI by running me333gui from the MATLAB command line<br />
# The user puts the proper COM port in the COM port text box and clicks Connect<br />
# The function ''Connect_Callback'' runs. This function pulls the text from COMport text box and sends it to the ''arm_connect'' function. The serial port is now open. The handle for the serial port object is stored in the UserData field.<br />
# The user puts the desired X and Y in the goto X and Y text boxes, selects the GoTo radio button, and clicks Start.<br />
# The function ''startButton_Callback''. This function checks which of the mode radio buttons are selected. If GoTo is selected, it reads from the X and Y boxes, and calls the ''arm_goto'' function with the COM port handle and the X and Y from the text box.<br />
# The arm receives the command and executes the move.<br />
<br />
==Results==<br />
[http://www.youtube.com/watch?v=Ffun2e6hnVE This video] demonstration shows the capabilities of the arm.<br />
<br />
<br />
Before paths are executed, the arm is lifted manually so that the optointerrupters are tripped and the arm is "homed," meaning that the PIC recognizes the arm's location with respect to motor encoder counts.<br />
<br />
The first path executed is a simple "Go To." <br />
<br />
[[Image: TraceTheta.jpg|thumb|150px|right]]<br />
After the "Go To," a looped "Trace" path is executed. The GUI screenshot at the right was used to generate this path. The top screenshot shows theta plotted against time and the bottom shows position plotted in xy space.<br />
[[Image: TraceXY.jpg|thumb|150px|right]]<br />
<br />
<br clear=all><br />
<br />
[[Image: CircleGUI.jpg|thumb|150px|right]]<br />
The last two paths are both circles. The first is a slow, wide circle rotating counter clockwise. The second is a quicker, tighter circle rotating clockwise. The GUI for the wide circle is at the right.<br />
<br />
<br clear=all><br />
<br />
==Next Steps==<br />
This project came a long way, but could go much further. <br />
<br />
One large area of improvement is in the logging of data. Logging speeds could be improved through more efficient coding, but even more through the use of USB communication. Since data are currently sent through RS-232, top transmission speeds are limited. Using USB would allow for the GUI graphs to update in much closer to real time. <br />
<br />
The motor control parameters could be tweaked to provide for greater control. Part of that is just trail and error for different constants, but better derivative and integral terms could be used in the control.<br />
<br />
Mechanical improvements would also aid in control. The arms currently move about and bounce (as seen in the video) due to motor backlash. Fixing this problem would allow for even more precise, quicker motions.<br />
<br />
The GUI could also be greatly improved to allow for more user options. The options were limited in this version to reduce the likelihood that a user would program a path fatal to the arm. With improved software and hardware limit switching, more user control would be available.<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=17982High Speed Motor Control2010-03-19T05:25:41Z<p>Sam Bobb: /* Overview */</p>
<hr />
<div>[[Image:2dofYoutubeLink.jpg|right|link = Results]]<br />
<br />
==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path.<br />
<br />
<br />
<math> x = L \cos (\theta_1)\ + \cos (\theta_1+\alpha)</math><br />
<br />
<math> y = L \sin(\theta_1)\ + \sin(\theta_1+\alpha)</math><br />
<br />
<math> \alpha = cos^{-1} \left(\frac{x^2+y^2-L^2-(2L)^2}{2L^2} \right)</math><br />
<br />
<math> \theta_1 = \frac{-(2 L \sin(\alpha)) x + (L + 2 L \cos(\alpha)) y} {(2 L \sin(\alpha)) y + (L + 2 L \cos(\alpha)) x}</math><br />
<br />
<math> \theta_2 = \theta_1 + \alpha \,</math><br />
<br />
<br />
Note: <math>\alpha\,</math> is used to calculate <math>\theta_{1,2}\,</math> in the MATLAB code and is not ever sent to the PIC.<br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
[[Image:2dof-interconnects.png|thumb|300px|Electrical block diagram.|right]]<br />
[[Image:2dof-hbridge.png|thumb|300px|H-bridge circuit and opto-isolators.|right]]<br />
<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
<br />
<br clear=all><br />
<br />
===Components===<br />
<br />
{| class="wikitable" border="1"<br />
|-<br />
! Electrical Components Needed.<br />
! Quantity<br />
! Data Sheets<br />
<br />
|-<br />
| PIC 32 on NU32 board<br />
| 1<br />
| [[Introduction to the PIC32|Introduction to the PIC32]]<br />
|-<br />
| H-bridges L298<br />
| 2<br />
| [http://www.st.com/stonline/books/pdf/docs/1773.pdf data sheet]<br />
|-<br />
| Optoisolators 4N27<br />
| 6<br />
| [http://media.digikey.com/pdf/Data%20Sheets/Lite-On%20PDFs/4N27_28_.pdf data sheet]<br />
|-<br />
| Quadrature Up/Down decoders LS7083<br />
| 2<br />
| [http://www.datasheetcatalog.com/datasheets_pdf/L/S/7/0/LS7083.shtml data sheet]<br />
|-<br />
| QVB11134 Optointerrupters<br />
| 2<br />
| [http://media.digikey.com/pdf/Data%20Sheets/Fairchild%20PDFs/QVB%20Series.pdf data sheet]<br />
|-<br />
| 35V 9A Schottkey Diodes 90SQ035-ND<br />
| 8<br />
| [http://www.vishay.com/doc?93417 data sheet]<br />
|-<br />
| Pittman GM8224 motor with 19.5:1 gearhead and 500 line encoder<br />
| 2<br />
| [[Media:pittmangearmotor.pdf|data sheet]]<br />
|-<br />
| 24VDC 6A power supply<br />
| 1<br />
| N/A<br />
|}<br />
<br />
===Circuit Diagram===<br />
<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[insert screenshot of gui]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii [CHECK THIS WITH SAM}. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in COLOR is the path that the PIC tried to execute, and the COLOR plot shows the actual path of the arm, for comparison. INCLUDE SOMETHING ABOUT THE GET DATA AND CONTINUOUS LOG BUTTONS!<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. This table shows the available commands. <br />
{| class="wikitable" border="1"<br />
|+Commands<br />
|-<br />
! Command !! Identifying Integer !! Description !! Number of following ints !! Syntax for following ints<br />
|-<br />
! C_HOLD<br />
| 0 || Hold arm in home position || 2 || [ x_position y_position ]<br />
|-<br />
! C_FLOAT<br />
| 1 || Stop and float motors || 1 || [ true = 1 ]<br />
|-<br />
! C_RUN<br />
| 2 || Run path || 1 || [ true = 1 ]<br />
|-<br />
! C_CONTROL_CONFIG<br />
| 3 || Send new control parameters || 14 || [ KPnum1 KPden1 KDnum1 KDden1 KInum1 KIden1 Kslope1 KPnum2 KPden2 KDnum2 KDden2 KInum2 KIden2 Kslope2 ]<br />
|-<br />
! C_LOG_CONFIG<br />
| 4 || Send new logging parameters (unimplemented) || 0 || --<br />
|-<br />
! C_SET_PATH<br />
| 5 || Send new path points || variable || [ n time<sub>1</sub> theta1<sub>1</sub> theta2<sub>1</sub> time<sub>2</sub> theta1<sub>2</sub> theta2<sub>2</sub> time<sub>3</sub> theta1<sub>3</sub> theta2<sub>3</sub> ... time<sub>n</sub> theta1<sub>n</sub> theta2<sub>n</sub> ]<br />
|-<br />
! C_SEND_LOG<br />
| 6 || Asks the PIC to send logged data || 0 || -- <br />
|-<br />
! C_LOOP<br />
| 7 || Sets the looping option || 1 || 0 = run once, 1 = loop the path <br />
|-<br />
! C_STARTSTOP<br />
| 8 || start and stop (unimplemented) || 0 || -- <br />
|-<br />
! C_GOTO<br />
| 9 || Tells the arm to goto a certain X,Y || 2 || [ X Y ]<br />
|-<br />
! C_PAUSE<br />
| 10 || Stop and hold at current location || 0 || --<br />
|-<br />
|}<br />
<br />
For example, the vector that MATLAB sends to move the arm to X = 15, Y = -10 would be: [9 15 -10].<br />
<br />
===PIC C Code===<br />
[[Media:2dof-arm-v3-PIC.zip]]<br />
<br />
This code runs on the PIC 32 and handles the motor control, control loop, logging, and serial interface. On start up, the PIC runs the initialization routines, and then waits for the user to move the arm through each of the optical break sensors. When a break is detected on one of the sensors, the position of the corresponding arm is set to zero. This allows an absolute position to be established for each arm. Once the procedure is complete, the PIC waits for commands from the PC.<br />
====Initialization====<br />
* mInitAllLEDs() -- sets up the on board LEDs<br />
* initPWMandIO() -- sets up digital IO pins for motors and more LEDs, sets up two analog to digital converters for the optical break sensors, configures the PWM modules and assigns them to Timer 3, and configures Timer3 at 20kHz<br />
* initEncoder() -- Sets up Timers 1, 2, 4, and 5 as external counters for the encoders.<br />
* initUART2(pbClk) -- Sets up UART2 for serial communication.<br />
====Timed loop (2 kHz)====<br />
What happens<br />
====Serial Interrupt====<br />
What happens<br />
<br />
===MATLAB Code===<br />
[[Media:2dof-arm-v3-matlab.zip]]<br />
<br />
The MATLAB library consists of functions for each of the commands that can be sent to the PIC and some helper functions. When one of the callback functions from the GUI runs, it pulls parameters from the interface, assembles them, and passes them to the proper command functions.<br />
<br />
Within the arm command functions, MATLAB uses the ''fwrite'' function to write data to the serial port. The ''int16'' option ensures that MATLAB breaks each number into a 16 bit integer and sends it as two bytes out the serial port. The PIC will then reassemble these two bytes as a 16 bit integer.<br />
<br />
====Example MATLAB procedure====<br />
Suppose the user wants move the arm to X = 15, Y = -10. The following happens:<br />
# The user opens the GUI by running me333gui from the MATLAB command line<br />
# The user puts the proper COM port in the COM port text box and clicks Connect<br />
# The function ''Connect_Callback'' runs. This function pulls the text from COMport text box and sends it to the ''arm_connect'' function. The serial port is now open. The handle for the serial port object is stored in the UserData field.<br />
# The user puts the desired X and Y in the goto X and Y text boxes, selects the GoTo radio button, and clicks Start.<br />
# The function ''startButton_Callback''. This function checks which of the mode radio buttons are selected. If GoTo is selected, it reads from the X and Y boxes, and calls the ''arm_goto'' function with the COM port handle and the X and Y from the text box.<br />
# The arm receives the command and executes the move.<br />
<br />
==Results==<br />
This video demonstration shows the capabilities of the arm.<br />
<br />
[[File: 2dofArmDemo.mp4|frame|none|alt=alt text| Demonstration of the arm.]]<br />
<br />
Before paths are executed, the arm is lifted manually so that the optointerrupters are tripped and the arm is "homed," meaning that the PIC recognizes the arm's location with respect to motor encoder counts.<br />
<br />
The first path executed is a simple "Go To." <br />
<br />
After the "Go To," a looped "Trace" path is executed. The GUI screenshot at the right was used to generate this path. The top screenshot shows theta plotted against time and the bottom shows position plotted in xy space.<br />
<br />
The last two paths are both circles. The first is a slow, wide circle rotating counter clockwise. The second is a quicker, tighter circle rotating clockwise. The GUI for the wide circle is at the right.<br />
<br />
==Next Steps==<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.<br />
<br />
==References==</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=17980High Speed Motor Control2010-03-19T05:25:07Z<p>Sam Bobb: /* Overview */</p>
<hr />
<div>[[Image:2dofYoutubeLink.jpg|right|link = Results]]<br />
<br />
==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path.<br />
<br />
<br />
<math> x = L \cos (\theta_1)\ + \cos (\theta_1+\alpha)</math><br />
<br />
<math> y = L \sin(\theta_1)\ + \sin(\theta_1+\alpha)</math><br />
<br />
<math> \alpha = cos^{-1} \left(\frac{x^2+y^2-L^2-(2L)^2}{2L^2} \right)</math><br />
<br />
<math> \theta_1 = \frac{-(2 L \sin(\alpha)) x + (L + 2 L \cos(\alpha)) y} {(2 L \sin(\alpha)) y + (L + 2 L \cos(\alpha)) x}</math><br />
<br />
<math> \theta_2 = \theta_1 + \alpha \,</math><br />
<br />
<br />
Note: <math>\alpha\,</math> is used to calculate <math>\theta_{1,2}\,</math> in the MATLAB code and is not ever sent to the PIC.<br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
[[Image:2dof-interconnects.png|thumb|300px|Electrical block diagram.|right]]<br />
[[Image:2dof-hbridge.png|thumb|300px|H-bridge circuit and opto-isolators.|right]]<br />
<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
===Components===<br />
<br />
{| class="wikitable" border="1"<br />
|-<br />
! Electrical Components Needed.<br />
! Quantity<br />
! Data Sheets<br />
<br />
|-<br />
| PIC 32 on NU32 board<br />
| 1<br />
| [[Introduction to the PIC32|Introduction to the PIC32]]<br />
|-<br />
| H-bridges L298<br />
| 2<br />
| [http://www.st.com/stonline/books/pdf/docs/1773.pdf data sheet]<br />
|-<br />
| Optoisolators 4N27<br />
| 6<br />
| [http://media.digikey.com/pdf/Data%20Sheets/Lite-On%20PDFs/4N27_28_.pdf data sheet]<br />
|-<br />
| Quadrature Up/Down decoders LS7083<br />
| 2<br />
| [http://www.datasheetcatalog.com/datasheets_pdf/L/S/7/0/LS7083.shtml data sheet]<br />
|-<br />
| QVB11134 Optointerrupters<br />
| 2<br />
| [http://media.digikey.com/pdf/Data%20Sheets/Fairchild%20PDFs/QVB%20Series.pdf data sheet]<br />
|-<br />
| 35V 9A Schottkey Diodes 90SQ035-ND<br />
| 8<br />
| [http://www.vishay.com/doc?93417 data sheet]<br />
|-<br />
| Pittman GM8224 motor with 19.5:1 gearhead and 500 line encoder<br />
| 2<br />
| [[Media:pittmangearmotor.pdf|data sheet]]<br />
|-<br />
| 24VDC 6A power supply<br />
| 1<br />
| N/A<br />
|}<br />
<br />
===Circuit Diagram===<br />
<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[insert screenshot of gui]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii [CHECK THIS WITH SAM}. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in COLOR is the path that the PIC tried to execute, and the COLOR plot shows the actual path of the arm, for comparison. INCLUDE SOMETHING ABOUT THE GET DATA AND CONTINUOUS LOG BUTTONS!<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. This table shows the available commands. <br />
{| class="wikitable" border="1"<br />
|+Commands<br />
|-<br />
! Command !! Identifying Integer !! Description !! Number of following ints !! Syntax for following ints<br />
|-<br />
! C_HOLD<br />
| 0 || Hold arm in home position || 2 || [ x_position y_position ]<br />
|-<br />
! C_FLOAT<br />
| 1 || Stop and float motors || 1 || [ true = 1 ]<br />
|-<br />
! C_RUN<br />
| 2 || Run path || 1 || [ true = 1 ]<br />
|-<br />
! C_CONTROL_CONFIG<br />
| 3 || Send new control parameters || 14 || [ KPnum1 KPden1 KDnum1 KDden1 KInum1 KIden1 Kslope1 KPnum2 KPden2 KDnum2 KDden2 KInum2 KIden2 Kslope2 ]<br />
|-<br />
! C_LOG_CONFIG<br />
| 4 || Send new logging parameters (unimplemented) || 0 || --<br />
|-<br />
! C_SET_PATH<br />
| 5 || Send new path points || variable || [ n time<sub>1</sub> theta1<sub>1</sub> theta2<sub>1</sub> time<sub>2</sub> theta1<sub>2</sub> theta2<sub>2</sub> time<sub>3</sub> theta1<sub>3</sub> theta2<sub>3</sub> ... time<sub>n</sub> theta1<sub>n</sub> theta2<sub>n</sub> ]<br />
|-<br />
! C_SEND_LOG<br />
| 6 || Asks the PIC to send logged data || 0 || -- <br />
|-<br />
! C_LOOP<br />
| 7 || Sets the looping option || 1 || 0 = run once, 1 = loop the path <br />
|-<br />
! C_STARTSTOP<br />
| 8 || start and stop (unimplemented) || 0 || -- <br />
|-<br />
! C_GOTO<br />
| 9 || Tells the arm to goto a certain X,Y || 2 || [ X Y ]<br />
|-<br />
! C_PAUSE<br />
| 10 || Stop and hold at current location || 0 || --<br />
|-<br />
|}<br />
<br />
For example, the vector that MATLAB sends to move the arm to X = 15, Y = -10 would be: [9 15 -10].<br />
<br />
===PIC C Code===<br />
[[Media:2dof-arm-v3-PIC.zip]]<br />
<br />
This code runs on the PIC 32 and handles the motor control, control loop, logging, and serial interface. On start up, the PIC runs the initialization routines, and then waits for the user to move the arm through each of the optical break sensors. When a break is detected on one of the sensors, the position of the corresponding arm is set to zero. This allows an absolute position to be established for each arm. Once the procedure is complete, the PIC waits for commands from the PC.<br />
====Initialization====<br />
* mInitAllLEDs() -- sets up the on board LEDs<br />
* initPWMandIO() -- sets up digital IO pins for motors and more LEDs, sets up two analog to digital converters for the optical break sensors, configures the PWM modules and assigns them to Timer 3, and configures Timer3 at 20kHz<br />
* initEncoder() -- Sets up Timers 1, 2, 4, and 5 as external counters for the encoders.<br />
* initUART2(pbClk) -- Sets up UART2 for serial communication.<br />
====Timed loop (2 kHz)====<br />
What happens<br />
====Serial Interrupt====<br />
What happens<br />
<br />
===MATLAB Code===<br />
[[Media:2dof-arm-v3-matlab.zip]]<br />
<br />
The MATLAB library consists of functions for each of the commands that can be sent to the PIC and some helper functions. When one of the callback functions from the GUI runs, it pulls parameters from the interface, assembles them, and passes them to the proper command functions.<br />
<br />
Within the arm command functions, MATLAB uses the ''fwrite'' function to write data to the serial port. The ''int16'' option ensures that MATLAB breaks each number into a 16 bit integer and sends it as two bytes out the serial port. The PIC will then reassemble these two bytes as a 16 bit integer.<br />
<br />
====Example MATLAB procedure====<br />
Suppose the user wants move the arm to X = 15, Y = -10. The following happens:<br />
# The user opens the GUI by running me333gui from the MATLAB command line<br />
# The user puts the proper COM port in the COM port text box and clicks Connect<br />
# The function ''Connect_Callback'' runs. This function pulls the text from COMport text box and sends it to the ''arm_connect'' function. The serial port is now open. The handle for the serial port object is stored in the UserData field.<br />
# The user puts the desired X and Y in the goto X and Y text boxes, selects the GoTo radio button, and clicks Start.<br />
# The function ''startButton_Callback''. This function checks which of the mode radio buttons are selected. If GoTo is selected, it reads from the X and Y boxes, and calls the ''arm_goto'' function with the COM port handle and the X and Y from the text box.<br />
# The arm receives the command and executes the move.<br />
<br />
==Results==<br />
This video demonstration shows the capabilities of the arm.<br />
<br />
[[File: 2dofArmDemo.mp4|frame|none|alt=alt text| Demonstration of the arm.]]<br />
<br />
Before paths are executed, the arm is lifted manually so that the optointerrupters are tripped and the arm is "homed," meaning that the PIC recognizes the arm's location with respect to motor encoder counts.<br />
<br />
The first path executed is a simple "Go To." <br />
<br />
After the "Go To," a looped "Trace" path is executed. The GUI screenshot at the right was used to generate this path. The top screenshot shows theta plotted against time and the bottom shows position plotted in xy space.<br />
<br />
The last two paths are both circles. The first is a slow, wide circle rotating counter clockwise. The second is a quicker, tighter circle rotating clockwise. The GUI for the wide circle is at the right.<br />
<br />
==Next Steps==<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.<br />
<br />
==References==</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=17977High Speed Motor Control2010-03-19T05:23:55Z<p>Sam Bobb: /* Overview */</p>
<hr />
<div>[[Image:2dofYoutubeLink.jpg|right|link = Results]]<br />
<br />
==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path.<br />
<br />
<br />
<math> x = L \cos (\theta_1)\ + \cos (\theta_1+\alpha)</math><br />
<br />
<math> y = L \sin(\theta_1)\ + \sin(\theta_1+\alpha)</math><br />
<br />
<math> \alpha = cos^{-1} \left(\frac{x^2+y^2-L^2-(2L)^2}{2L^2} \right)</math><br />
<br />
<math> \theta_1 = \frac{-(2 L \sin(\alpha)) x + (L + 2 L \cos(\alpha)) y} {(2 L \sin(\alpha)) y + (L + 2 L \cos(\alpha)) x}</math><br />
<br />
<math> \theta_2 = \theta_1 + \alpha \,</math><br />
<br />
<br />
Note: <math>\alpha\,</math> is used to calculate <math>\theta_{1,2}\,</math> in the MATLAB code and is not ever sent to the PIC.<br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
[[Image:2dof-interconnects.png]]<br />
[[Image:2dof-hbridge.png]]<br />
<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
===Components===<br />
<br />
{| class="wikitable" border="1"<br />
|-<br />
! Electrical Components Needed.<br />
! Quantity<br />
! Data Sheets<br />
<br />
|-<br />
| PIC 32 on NU32 board<br />
| 1<br />
| [[Introduction to the PIC32|Introduction to the PIC32]]<br />
|-<br />
| H-bridges L298<br />
| 2<br />
| [http://www.st.com/stonline/books/pdf/docs/1773.pdf data sheet]<br />
|-<br />
| Optoisolators 4N27<br />
| 6<br />
| [http://media.digikey.com/pdf/Data%20Sheets/Lite-On%20PDFs/4N27_28_.pdf data sheet]<br />
|-<br />
| Quadrature Up/Down decoders LS7083<br />
| 2<br />
| [http://www.datasheetcatalog.com/datasheets_pdf/L/S/7/0/LS7083.shtml data sheet]<br />
|-<br />
| QVB11134 Optointerrupters<br />
| 2<br />
| [http://media.digikey.com/pdf/Data%20Sheets/Fairchild%20PDFs/QVB%20Series.pdf data sheet]<br />
|-<br />
| 35V 9A Schottkey Diodes 90SQ035-ND<br />
| 8<br />
| [http://www.vishay.com/doc?93417 data sheet]<br />
|-<br />
| Pittman GM8224 motor with 19.5:1 gearhead and 500 line encoder<br />
| 2<br />
| [[Media:pittmangearmotor.pdf|data sheet]]<br />
|-<br />
| 24VDC 6A power supply<br />
| 1<br />
| N/A<br />
|}<br />
<br />
===Circuit Diagram===<br />
<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[insert screenshot of gui]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii [CHECK THIS WITH SAM}. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in COLOR is the path that the PIC tried to execute, and the COLOR plot shows the actual path of the arm, for comparison. INCLUDE SOMETHING ABOUT THE GET DATA AND CONTINUOUS LOG BUTTONS!<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. This table shows the available commands. <br />
{| class="wikitable" border="1"<br />
|+Commands<br />
|-<br />
! Command !! Identifying Integer !! Description !! Number of following ints !! Syntax for following ints<br />
|-<br />
! C_HOLD<br />
| 0 || Hold arm in home position || 2 || [ x_position y_position ]<br />
|-<br />
! C_FLOAT<br />
| 1 || Stop and float motors || 1 || [ true = 1 ]<br />
|-<br />
! C_RUN<br />
| 2 || Run path || 1 || [ true = 1 ]<br />
|-<br />
! C_CONTROL_CONFIG<br />
| 3 || Send new control parameters || 14 || [ KPnum1 KPden1 KDnum1 KDden1 KInum1 KIden1 Kslope1 KPnum2 KPden2 KDnum2 KDden2 KInum2 KIden2 Kslope2 ]<br />
|-<br />
! C_LOG_CONFIG<br />
| 4 || Send new logging parameters (unimplemented) || 0 || --<br />
|-<br />
! C_SET_PATH<br />
| 5 || Send new path points || variable || [ n time<sub>1</sub> theta1<sub>1</sub> theta2<sub>1</sub> time<sub>2</sub> theta1<sub>2</sub> theta2<sub>2</sub> time<sub>3</sub> theta1<sub>3</sub> theta2<sub>3</sub> ... time<sub>n</sub> theta1<sub>n</sub> theta2<sub>n</sub> ]<br />
|-<br />
! C_SEND_LOG<br />
| 6 || Asks the PIC to send logged data || 0 || -- <br />
|-<br />
! C_LOOP<br />
| 7 || Sets the looping option || 1 || 0 = run once, 1 = loop the path <br />
|-<br />
! C_STARTSTOP<br />
| 8 || start and stop (unimplemented) || 0 || -- <br />
|-<br />
! C_GOTO<br />
| 9 || Tells the arm to goto a certain X,Y || 2 || [ X Y ]<br />
|-<br />
! C_PAUSE<br />
| 10 || Stop and hold at current location || 0 || --<br />
|-<br />
|}<br />
<br />
For example, the vector that MATLAB sends to move the arm to X = 15, Y = -10 would be: [9 15 -10].<br />
<br />
===PIC C Code===<br />
[[Media:2dof-arm-v3-PIC.zip]]<br />
<br />
This code runs on the PIC 32 and handles the motor control, control loop, logging, and serial interface. On start up, the PIC runs the initialization routines, and then waits for the user to move the arm through each of the optical break sensors. When a break is detected on one of the sensors, the position of the corresponding arm is set to zero. This allows an absolute position to be established for each arm. Once the procedure is complete, the PIC waits for commands from the PC.<br />
====Initialization====<br />
* mInitAllLEDs() -- sets up the on board LEDs<br />
* initPWMandIO() -- sets up digital IO pins for motors and more LEDs, sets up two analog to digital converters for the optical break sensors, configures the PWM modules and assigns them to Timer 3, and configures Timer3 at 20kHz<br />
* initEncoder() -- Sets up Timers 1, 2, 4, and 5 as external counters for the encoders.<br />
* initUART2(pbClk) -- Sets up UART2 for serial communication.<br />
====Timed loop (2 kHz)====<br />
What happens<br />
====Serial Interrupt====<br />
What happens<br />
<br />
===MATLAB Code===<br />
[[Media:2dof-arm-v3-matlab.zip]]<br />
<br />
The MATLAB library consists of functions for each of the commands that can be sent to the PIC and some helper functions. When one of the callback functions from the GUI runs, it pulls parameters from the interface, assembles them, and passes them to the proper command functions.<br />
<br />
Within the arm command functions, MATLAB uses the ''fwrite'' function to write data to the serial port. The ''int16'' option ensures that MATLAB breaks each number into a 16 bit integer and sends it as two bytes out the serial port. The PIC will then reassemble these two bytes as a 16 bit integer.<br />
<br />
====Example MATLAB procedure====<br />
Suppose the user wants move the arm to X = 15, Y = -10. The following happens:<br />
# The user opens the GUI by running me333gui from the MATLAB command line<br />
# The user puts the proper COM port in the COM port text box and clicks Connect<br />
# The function ''Connect_Callback'' runs. This function pulls the text from COMport text box and sends it to the ''arm_connect'' function. The serial port is now open. The handle for the serial port object is stored in the UserData field.<br />
# The user puts the desired X and Y in the goto X and Y text boxes, selects the GoTo radio button, and clicks Start.<br />
# The function ''startButton_Callback''. This function checks which of the mode radio buttons are selected. If GoTo is selected, it reads from the X and Y boxes, and calls the ''arm_goto'' function with the COM port handle and the X and Y from the text box.<br />
# The arm receives the command and executes the move.<br />
<br />
==Results==<br />
This video demonstration shows the capabilities of the arm.<br />
<br />
Before paths are executed, the arm is lifted manually so that the optointerrupters are tripped and the arm is "homed," meaning that the PIC recognizes the arm's location with respect to motor encoder counts.<br />
<br />
The first path executed is a simple "Go To." <br />
<br />
After the "Go To," a looped "Trace" path is executed. The GUI screenshot at the right was used to generate this path. The top screenshot shows theta plotted against time and the bottom shows position plotted in xy space.<br />
<br />
The last two paths are both circles. The first is a slow, wide circle rotating counter clockwise. The second is a quicker, tighter circle rotating clockwise. The GUI for the wide circle is at the right.<br />
<br />
==Next Steps==<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.<br />
<br />
==References==</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=File:2dof-hbridge.png&diff=17976File:2dof-hbridge.png2010-03-19T05:23:29Z<p>Sam Bobb: </p>
<hr />
<div></div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=17959High Speed Motor Control2010-03-19T04:41:43Z<p>Sam Bobb: /* Overview */</p>
<hr />
<div>[[Image:2dofYoutubeLink.jpg|right|link = Results]]<br />
<br />
==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path.<br />
<br />
<br />
<math> x = L \cos (\theta_1)\ + \cos (\theta_1+\alpha)</math><br />
<br />
<math> y = L \sin(\theta_1)\ + \sin(\theta_1+\alpha)</math><br />
<br />
<math> \alpha = cos^{-1} \left(\frac{x^2+y^2-L^2-(2L)^2}{2L^2} \right)</math><br />
<br />
<math> \theta_1 = \frac{-(2 L \sin(\alpha)) x + (L + 2 L \cos(\alpha)) y} {(2 L \sin(\alpha)) y + (L + 2 L \cos(\alpha)) x}</math><br />
<br />
<math> \theta_2 = \theta_1 + \alpha \,</math><br />
<br />
<br />
Note: <math>\alpha\,</math> is used to calculate <math>\theta_{1,2}\,</math> in the MATLAB code and is not ever sent to the PIC.<br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
[[Image:2dof-interconnects.png]]<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
===Components===<br />
<br />
{| class="wikitable" border="1"<br />
|-<br />
! Electrical Components Needed.<br />
! Quantity<br />
! Data Sheets<br />
<br />
|-<br />
| PIC 32 on NU32 board<br />
| 1<br />
| [[Introduction to the PIC32|Introduction to the PIC32]]<br />
|-<br />
| H-bridges L298<br />
| 2<br />
| [http://www.st.com/stonline/books/pdf/docs/1773.pdf data sheet]<br />
|-<br />
| Optoisolators 4N27<br />
| 6<br />
|<br />
|-<br />
| Quadrature Up/Down decoders LS7083<br />
| 2<br />
| [http://www.datasheetcatalog.com/datasheets_pdf/L/S/7/0/LS7083.shtml data sheet]<br />
|-<br />
| QVB11134 Optointerrupters<br />
| 2<br />
| [http://media.digikey.com/pdf/Data%20Sheets/Fairchild%20PDFs/QVB%20Series.pdf data sheet]<br />
|-<br />
| 35V 9A Schottkey Diodes 90SQ035-ND<br />
| 8<br />
| [http://www.vishay.com/doc?93417 data sheet]<br />
|-<br />
| Pittman GM8224 motor with 19.5:1 gearhead and 500 line encoder<br />
| 2<br />
| [[Media:pittmangearmotor.pdf|data sheet]]<br />
|-<br />
| 24VDC 6A power supply<br />
| 1<br />
| N/A<br />
|}<br />
<br />
===Circuit Diagram===<br />
<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[insert screenshot of gui]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii [CHECK THIS WITH SAM}. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in COLOR is the path that the PIC tried to execute, and the COLOR plot shows the actual path of the arm, for comparison. INCLUDE SOMETHING ABOUT THE GET DATA AND CONTINUOUS LOG BUTTONS!<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. This table shows the available commands. <br />
{| class="wikitable" border="1"<br />
|+Commands<br />
|-<br />
! Command !! Identifying Integer !! Description !! Number of following ints !! Syntax for following ints<br />
|-<br />
! C_HOLD<br />
| 0 || Hold arm in home position || 2 || [ x_position y_position ]<br />
|-<br />
! C_FLOAT<br />
| 1 || Stop and float motors || 1 || [ true = 1 ]<br />
|-<br />
! C_RUN<br />
| 2 || Run path || 1 || [ true = 1 ]<br />
|-<br />
! C_CONTROL_CONFIG<br />
| 3 || Send new control parameters || 14 || [ KPnum1 KPden1 KDnum1 KDden1 KInum1 KIden1 Kslope1 KPnum2 KPden2 KDnum2 KDden2 KInum2 KIden2 Kslope2 ]<br />
|-<br />
! C_LOG_CONFIG<br />
| 4 || Send new logging parameters (unimplemented) || 0 || --<br />
|-<br />
! C_SET_PATH<br />
| 5 || Send new path points || variable || [ n time<sub>1</sub> theta1<sub>1</sub> theta2<sub>1</sub> time<sub>2</sub> theta1<sub>2</sub> theta2<sub>2</sub> time<sub>3</sub> theta1<sub>3</sub> theta2<sub>3</sub> ... time<sub>n</sub> theta1<sub>n</sub> theta2<sub>n</sub> ]<br />
|-<br />
! C_SEND_LOG<br />
| 6 || Asks the PIC to send logged data || 0 || -- <br />
|-<br />
! C_LOOP<br />
| 7 || Sets the looping option || 1 || 0 = run once, 1 = loop the path <br />
|-<br />
! C_STARTSTOP<br />
| 8 || start and stop (unimplemented) || 0 || -- <br />
|-<br />
! C_GOTO<br />
| 9 || Tells the arm to goto a certain X,Y || 2 || [ X Y ]<br />
|-<br />
! C_PAUSE<br />
| 10 || Stop and hold at current location || 0 || --<br />
|-<br />
|}<br />
<br />
For example, the vector that MATLAB sends to move the arm to X = 15, Y = -10 would be: [9 15 -10].<br />
<br />
===PIC C Code===<br />
[[Media:2dof-arm-v3-PIC.zip]]<br />
<br />
This code runs on the PIC 32 and handles the motor control, control loop, logging, and serial interface. On start up, the PIC runs the initialization routines, and then waits for the user to move the arm through each of the optical break sensors. When a break is detected on one of the sensors, the position of the corresponding arm is set to zero. This allows an absolute position to be established for each arm. Once the procedure is complete, the PIC waits for commands from the PC.<br />
====Initialization====<br />
* mInitAllLEDs() -- sets up the on board LEDs<br />
* initPWMandIO() -- sets up digital IO pins for motors and more LEDs, sets up two analog to digital converters for the optical break sensors, configures the PWM modules and assigns them to Timer 3, and configures Timer3 at 20kHz<br />
* initEncoder() -- Sets up Timers 1, 2, 4, and 5 as external counters for the encoders.<br />
* initUART2(pbClk) -- Sets up UART2 for serial communication.<br />
====Timed loop (2 kHz)====<br />
What happens<br />
====Serial Interrupt====<br />
What happens<br />
<br />
===MATLAB Code===<br />
[[Media:2dof-arm-v3-matlab.zip]]<br />
<br />
The MATLAB library consists of functions for each of the commands that can be sent to the PIC and some helper functions. When one of the callback functions from the GUI runs, it pulls parameters from the interface, assembles them, and passes them to the proper command functions.<br />
<br />
Within the arm command functions, MATLAB uses the ''fwrite'' function to write data to the serial port. The ''int16'' option ensures that MATLAB breaks each number into a 16 bit integer and sends it as two bytes out the serial port. The PIC will then reassemble these two bytes as a 16 bit integer.<br />
<br />
====Example MATLAB procedure====<br />
Suppose the user wants move the arm to X = 15, Y = -10. The following happens:<br />
# The user opens the GUI by running me333gui from the MATLAB command line<br />
# The user puts the proper COM port in the COM port text box and clicks Connect<br />
# The function ''Connect_Callback'' runs. This function pulls the text from COMport text box and sends it to the ''arm_connect'' function. The serial port is now open. The handle for the serial port object is stored in the UserData field.<br />
# The user puts the desired X and Y in the goto X and Y text boxes, selects the GoTo radio button, and clicks Start.<br />
# The function ''startButton_Callback''. This function checks which of the mode radio buttons are selected. If GoTo is selected, it reads from the X and Y boxes, and calls the ''arm_goto'' function with the COM port handle and the X and Y from the text box.<br />
# The arm receives the command and executes the move.<br />
<br />
==Results==<br />
It was awesome.<br />
==Next Steps==<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.<br />
<br />
==References==</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=File:2dof-interconnects.png&diff=17958File:2dof-interconnects.png2010-03-19T04:41:20Z<p>Sam Bobb: </p>
<hr />
<div></div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=17915High Speed Motor Control2010-03-19T03:03:55Z<p>Sam Bobb: /* MATLAB Code */</p>
<hr />
<div>==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path.<br />
<br />
<br />
<math> x = L \cos (\theta_1)\ + \cos (\theta_1+\alpha)</math><br />
<br />
<math> y = L \sin(\theta_1)\ + \sin(\theta_1+\alpha)</math><br />
<br />
<math> \alpha = cos^{-1} \left(\frac{x^2+y^2-L^2-(2L)^2}{2L^2} \right)</math><br />
<br />
<math> \theta_1 = \frac{-(2 L \sin(\alpha)) x + (L + 2 L \cos(\alpha)) y} {(2 L \sin(\alpha)) y + (L + 2 L \cos(\alpha)) x}</math><br />
<br />
<math> \theta_2 = \theta_1 + \alpha \,</math><br />
<br />
<br />
Note: <math>\alpha\,</math> is used to calculate <math>\theta_{1,2}\,</math> in the MATLAB code and is not ever sent to the PIC.<br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
===Components===<br />
===Circuit Diagram===<br />
<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[insert screenshot of gui]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii [CHECK THIS WITH SAM}. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in COLOR is the path that the PIC tried to execute, and the COLOR plot shows the actual path of the arm, for comparison. INCLUDE SOMETHING ABOUT THE GET DATA AND CONTINUOUS LOG BUTTONS!<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. This table shows the available commands. <br />
{| class="wikitable" border="1"<br />
|+Commands<br />
|-<br />
! Command !! Identifying Integer !! Description !! Number of following ints !! Syntax for following ints<br />
|-<br />
! C_HOLD<br />
| 0 || Hold arm in home position || 2 || [ x_position y_position ]<br />
|-<br />
! C_FLOAT<br />
| 1 || Stop and float motors || 1 || [ true = 1 ]<br />
|-<br />
! C_RUN<br />
| 2 || Run path || 1 || [ true = 1 ]<br />
|-<br />
! C_CONTROL_CONFIG<br />
| 3 || Send new control parameters || 14 || [ KPnum1 KPden1 KDnum1 KDden1 KInum1 KIden1 Kslope1 KPnum2 KPden2 KDnum2 KDden2 KInum2 KIden2 Kslope2 ]<br />
|-<br />
! C_LOG_CONFIG<br />
| 4 || Send new logging parameters (unimplemented) || 0 || --<br />
|-<br />
! C_SET_PATH<br />
| 5 || Send new path points || variable || [ n time<sub>1</sub> theta1<sub>1</sub> theta2<sub>1</sub> time<sub>2</sub> theta1<sub>2</sub> theta2<sub>2</sub> time<sub>3</sub> theta1<sub>3</sub> theta2<sub>3</sub> ... time<sub>n</sub> theta1<sub>n</sub> theta2<sub>n</sub> ]<br />
|-<br />
! C_SEND_LOG<br />
| 6 || Asks the PIC to send logged data || 0 || -- <br />
|-<br />
! C_LOOP<br />
| 7 || Sets the looping option || 1 || 0 = run once, 1 = loop the path <br />
|-<br />
! C_STARTSTOP<br />
| 8 || start and stop (unimplemented) || 0 || -- <br />
|-<br />
! C_GOTO<br />
| 9 || Tells the arm to goto a certain X,Y || 2 || [ X Y ]<br />
|-<br />
! C_PAUSE<br />
| 10 || Stop and hold at current location || 0 || --<br />
|-<br />
|}<br />
<br />
For example, the vector that MATLAB sends to move the arm to X = 15, Y = -10 would be: [9 15 -10].<br />
<br />
===PIC C Code===<br />
[[Media:2dof-arm-v3-PIC.zip]]<br />
<br />
This code runs on the PIC 32 and handles the motor control, control loop, logging, and serial interface. On start up, the PIC runs the initialization routines, and then waits for the user to move the arm through each of the optical break sensors. When a break is detected on one of the sensors, the position of the corresponding arm is set to zero. This allows an absolute position to be established for each arm. Once the procedure is complete, the PIC waits for commands from the PC.<br />
====Initialization====<br />
* mInitAllLEDs() -- sets up the on board LEDs<br />
* initPWMandIO() -- sets up digital IO pins for motors and more LEDs, sets up two analog to digital converters for the optical break sensors, configures the PWM modules and assigns them to Timer 3, and configures Timer3 at 20kHz<br />
* initEncoder() -- Sets up Timers 1, 2, 4, and 5 as external counters for the encoders.<br />
* initUART2(pbClk) -- Sets up UART2 for serial communication.<br />
====Timed loop (2 kHz)====<br />
What happens<br />
====Serial Interrupt====<br />
What happens<br />
<br />
===MATLAB Code===<br />
[[Media:2dof-arm-v3-matlab.zip]]<br />
<br />
The MATLAB library consists of functions for each of the commands that can be sent to the PIC and some helper functions. When one of the callback functions from the GUI runs, it pulls parameters from the interface, assembles them, and passes them to the proper command functions.<br />
<br />
Within the arm command functions, MATLAB uses the ''fwrite'' function to write data to the serial port. The ''int16'' option ensures that MATLAB breaks each number into a 16 bit integer and sends it as two bytes out the serial port. The PIC will then reassemble these two bytes as a 16 bit integer.<br />
<br />
====Example MATLAB procedure====<br />
Suppose the user wants move the arm to X = 15, Y = -10. The following happens:<br />
# The user opens the GUI by running me333gui from the MATLAB command line<br />
# The user puts the proper COM port in the COM port text box and clicks Connect<br />
# The function ''Connect_Callback'' runs. This function pulls the text from COMport text box and sends it to the ''arm_connect'' function. The serial port is now open. The handle for the serial port object is stored in the UserData field.<br />
# The user puts the desired X and Y in the goto X and Y text boxes, selects the GoTo radio button, and clicks Start.<br />
# The function ''startButton_Callback''. This function checks which of the mode radio buttons are selected. If GoTo is selected, it reads from the X and Y boxes, and calls the ''arm_goto'' function with the COM port handle and the X and Y from the text box.<br />
# The arm receives the command and executes the move.<br />
<br />
==Results==<br />
It was awesome.<br />
==Next Steps==<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.<br />
<br />
==References==</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=17914High Speed Motor Control2010-03-19T03:01:38Z<p>Sam Bobb: /* MATLAB Code */</p>
<hr />
<div>==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path.<br />
<br />
<br />
<math> x = L \cos (\theta_1)\ + \cos (\theta_1+\alpha)</math><br />
<br />
<math> y = L \sin(\theta_1)\ + \sin(\theta_1+\alpha)</math><br />
<br />
<math> \alpha = cos^{-1} \left(\frac{x^2+y^2-L^2-(2L)^2}{2L^2} \right)</math><br />
<br />
<math> \theta_1 = \frac{-(2 L \sin(\alpha)) x + (L + 2 L \cos(\alpha)) y} {(2 L \sin(\alpha)) y + (L + 2 L \cos(\alpha)) x}</math><br />
<br />
<math> \theta_2 = \theta_1 + \alpha \,</math><br />
<br />
<br />
Note: <math>\alpha\,</math> is used to calculate <math>\theta_{1,2}\,</math> in the MATLAB code and is not ever sent to the PIC.<br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
===Components===<br />
===Circuit Diagram===<br />
<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[insert screenshot of gui]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii [CHECK THIS WITH SAM}. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in COLOR is the path that the PIC tried to execute, and the COLOR plot shows the actual path of the arm, for comparison. INCLUDE SOMETHING ABOUT THE GET DATA AND CONTINUOUS LOG BUTTONS!<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. This table shows the available commands. <br />
{| class="wikitable" border="1"<br />
|+Commands<br />
|-<br />
! Command !! Identifying Integer !! Description !! Number of following ints !! Syntax for following ints<br />
|-<br />
! C_HOLD<br />
| 0 || Hold arm in home position || 2 || [ x_position y_position ]<br />
|-<br />
! C_FLOAT<br />
| 1 || Stop and float motors || 1 || [ true = 1 ]<br />
|-<br />
! C_RUN<br />
| 2 || Run path || 1 || [ true = 1 ]<br />
|-<br />
! C_CONTROL_CONFIG<br />
| 3 || Send new control parameters || 14 || [ KPnum1 KPden1 KDnum1 KDden1 KInum1 KIden1 Kslope1 KPnum2 KPden2 KDnum2 KDden2 KInum2 KIden2 Kslope2 ]<br />
|-<br />
! C_LOG_CONFIG<br />
| 4 || Send new logging parameters (unimplemented) || 0 || --<br />
|-<br />
! C_SET_PATH<br />
| 5 || Send new path points || variable || [ n time<sub>1</sub> theta1<sub>1</sub> theta2<sub>1</sub> time<sub>2</sub> theta1<sub>2</sub> theta2<sub>2</sub> time<sub>3</sub> theta1<sub>3</sub> theta2<sub>3</sub> ... time<sub>n</sub> theta1<sub>n</sub> theta2<sub>n</sub> ]<br />
|-<br />
! C_SEND_LOG<br />
| 6 || Asks the PIC to send logged data || 0 || -- <br />
|-<br />
! C_LOOP<br />
| 7 || Sets the looping option || 1 || 0 = run once, 1 = loop the path <br />
|-<br />
! C_STARTSTOP<br />
| 8 || start and stop (unimplemented) || 0 || -- <br />
|-<br />
! C_GOTO<br />
| 9 || Tells the arm to goto a certain X,Y || 2 || [ X Y ]<br />
|-<br />
! C_PAUSE<br />
| 10 || Stop and hold at current location || 0 || --<br />
|-<br />
|}<br />
<br />
For example, the vector that MATLAB sends to move the arm to X = 15, Y = -10 would be: [9 15 -10].<br />
<br />
===PIC C Code===<br />
[[Media:2dof-arm-v3-PIC.zip]]<br />
<br />
This code runs on the PIC 32 and handles the motor control, control loop, logging, and serial interface. On start up, the PIC runs the initialization routines, and then waits for the user to move the arm through each of the optical break sensors. When a break is detected on one of the sensors, the position of the corresponding arm is set to zero. This allows an absolute position to be established for each arm. Once the procedure is complete, the PIC waits for commands from the PC.<br />
====Initialization====<br />
* mInitAllLEDs() -- sets up the on board LEDs<br />
* initPWMandIO() -- sets up digital IO pins for motors and more LEDs, sets up two analog to digital converters for the optical break sensors, configures the PWM modules and assigns them to Timer 3, and configures Timer3 at 20kHz<br />
* initEncoder() -- Sets up Timers 1, 2, 4, and 5 as external counters for the encoders.<br />
* initUART2(pbClk) -- Sets up UART2 for serial communication.<br />
====Timed loop (2 kHz)====<br />
What happens<br />
====Serial Interrupt====<br />
What happens<br />
<br />
===MATLAB Code===<br />
[[Media:2dof-arm-v3-matlab.zip]]<br />
<br />
The MATLAB library consists of functions for each of the commands that can be sent to the PIC and some helper functions. When one of the callback functions from the GUI runs, it pulls parameters from the interface, assembles them, and passes them to the proper command functions.<br />
<br />
====Example MATLAB procedure====<br />
Suppose the user wants move the arm to X = 15, Y = -10. The following happens:<br />
# The user opens the GUI by running me333gui from the MATLAB command line<br />
# The user puts the proper COM port in the COM port text box and clicks Connect<br />
# The function ''Connect_Callback'' runs. This function pulls the text from COMport text box and sends it to the ''arm_connect'' function. The serial port is now open. The handle for the serial port object is stored in the UserData field.<br />
# The user puts the desired X and Y in the goto X and Y text boxes, selects the GoTo radio button, and clicks Start.<br />
# The function ''startButton_Callback''. This function checks which of the mode radio buttons are selected. If GoTo is selected, it reads from the X and Y boxes, and calls the ''arm_goto'' function with the COM port handle and the X and Y from the text box.<br />
# The arm receives the command and executes the move.<br />
<br />
==Results==<br />
It was awesome.<br />
==Next Steps==<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.<br />
<br />
==References==</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=17912High Speed Motor Control2010-03-19T02:47:28Z<p>Sam Bobb: /* PIC C Code */</p>
<hr />
<div>==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path.<br />
<br />
<br />
<math> x = L \cos (\theta_1)\ + \cos (\theta_1+\alpha)</math><br />
<br />
<math> y = L \sin(\theta_1)\ + \sin(\theta_1+\alpha)</math><br />
<br />
<math> \alpha = cos^{-1} \left(\frac{x^2+y^2-L^2-(2L)^2}{2L^2} \right)</math><br />
<br />
<math> \theta_1 = \frac{-(2 L \sin(\alpha)) x + (L + 2 L \cos(\alpha)) y} {(2 L \sin(\alpha)) y + (L + 2 L \cos(\alpha)) x}</math><br />
<br />
<math> \theta_2 = \theta_1 + \alpha \,</math><br />
<br />
<br />
Note: <math>\alpha\,</math> is used to calculate <math>\theta_{1,2}\,</math> in the MATLAB code and is not ever sent to the PIC.<br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
===Components===<br />
===Circuit Diagram===<br />
<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[insert screenshot of gui]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii [CHECK THIS WITH SAM}. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in COLOR is the path that the PIC tried to execute, and the COLOR plot shows the actual path of the arm, for comparison. INCLUDE SOMETHING ABOUT THE GET DATA AND CONTINUOUS LOG BUTTONS!<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. This table shows the available commands. <br />
{| class="wikitable" border="1"<br />
|+Commands<br />
|-<br />
! Command !! Identifying Integer !! Description !! Number of following ints !! Syntax for following ints<br />
|-<br />
! C_HOLD<br />
| 0 || Hold arm in home position || 2 || [ x_position y_position ]<br />
|-<br />
! C_FLOAT<br />
| 1 || Stop and float motors || 1 || [ true = 1 ]<br />
|-<br />
! C_RUN<br />
| 2 || Run path || 1 || [ true = 1 ]<br />
|-<br />
! C_CONTROL_CONFIG<br />
| 3 || Send new control parameters || 14 || [ KPnum1 KPden1 KDnum1 KDden1 KInum1 KIden1 Kslope1 KPnum2 KPden2 KDnum2 KDden2 KInum2 KIden2 Kslope2 ]<br />
|-<br />
! C_LOG_CONFIG<br />
| 4 || Send new logging parameters (unimplemented) || 0 || --<br />
|-<br />
! C_SET_PATH<br />
| 5 || Send new path points || variable || [ n time<sub>1</sub> theta1<sub>1</sub> theta2<sub>1</sub> time<sub>2</sub> theta1<sub>2</sub> theta2<sub>2</sub> time<sub>3</sub> theta1<sub>3</sub> theta2<sub>3</sub> ... time<sub>n</sub> theta1<sub>n</sub> theta2<sub>n</sub> ]<br />
|-<br />
! C_SEND_LOG<br />
| 6 || Asks the PIC to send logged data || 0 || -- <br />
|-<br />
! C_LOOP<br />
| 7 || Sets the looping option || 1 || 0 = run once, 1 = loop the path <br />
|-<br />
! C_STARTSTOP<br />
| 8 || start and stop (unimplemented) || 0 || -- <br />
|-<br />
! C_GOTO<br />
| 9 || Tells the arm to goto a certain X,Y || 2 || [ X Y ]<br />
|-<br />
! C_PAUSE<br />
| 10 || Stop and hold at current location || 0 || --<br />
|-<br />
|}<br />
<br />
For example, the vector that MATLAB sends to move the arm to X = 15, Y = -10 would be: [9 15 -10].<br />
<br />
===PIC C Code===<br />
[[Media:2dof-arm-v3-PIC.zip]]<br />
<br />
This code runs on the PIC 32 and handles the motor control, control loop, logging, and serial interface. On start up, the PIC runs the initialization routines, and then waits for the user to move the arm through each of the optical break sensors. When a break is detected on one of the sensors, the position of the corresponding arm is set to zero. This allows an absolute position to be established for each arm. Once the procedure is complete, the PIC waits for commands from the PC.<br />
====Initialization====<br />
* mInitAllLEDs() -- sets up the on board LEDs<br />
* initPWMandIO() -- sets up digital IO pins for motors and more LEDs, sets up two analog to digital converters for the optical break sensors, configures the PWM modules and assigns them to Timer 3, and configures Timer3 at 20kHz<br />
* initEncoder() -- Sets up Timers 1, 2, 4, and 5 as external counters for the encoders.<br />
* initUART2(pbClk) -- Sets up UART2 for serial communication.<br />
====Timed loop (2 kHz)====<br />
What happens<br />
====Serial Interrupt====<br />
What happens<br />
<br />
===MATLAB Code===<br />
[[Media:2dof-arm-v3-matlab.zip]]<br />
<br />
==Results==<br />
It was awesome.<br />
==Next Steps==<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.<br />
<br />
==References==</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=17910High Speed Motor Control2010-03-19T02:26:21Z<p>Sam Bobb: /* Overview */</p>
<hr />
<div>==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path.<br />
<br />
<br />
<math> x = L \cos (\theta_1)\ + \cos (\theta_1+\alpha)</math><br />
<br />
<math> y = L \sin(\theta_1)\ + \sin(\theta_1+\alpha)</math><br />
<br />
<math> \alpha = cos^{-1} \left(\frac{x^2+y^2-L^2-(2L)^2}{2L^2} \right)</math><br />
<br />
<math> \theta_1 = \frac{-(2 L \sin(\alpha)) x + (L + 2 L \cos(\alpha)) y} {(2 L \sin(\alpha)) y + (L + 2 L \cos(\alpha)) x}</math><br />
<br />
<math> \theta_2 = \theta_1 + \alpha \,</math><br />
<br />
<br />
Note: <math>\alpha\,</math> is used to calculate <math>\theta_{1,2}\,</math> in the MATLAB code and is not ever sent to the PIC.<br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
===Components===<br />
===Circuit Diagram===<br />
<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[insert screenshot of gui]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii [CHECK THIS WITH SAM}. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in COLOR is the path that the PIC tried to execute, and the COLOR plot shows the actual path of the arm, for comparison. INCLUDE SOMETHING ABOUT THE GET DATA AND CONTINUOUS LOG BUTTONS!<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. This table shows the available commands. <br />
{| class="wikitable" border="1"<br />
|+Commands<br />
|-<br />
! Command !! Identifying Integer !! Description !! Number of following ints !! Syntax for following ints<br />
|-<br />
! C_HOLD<br />
| 0 || Hold arm in home position || 2 || [ x_position y_position ]<br />
|-<br />
! C_FLOAT<br />
| 1 || Stop and float motors || 1 || [ true = 1 ]<br />
|-<br />
! C_RUN<br />
| 2 || Run path || 1 || [ true = 1 ]<br />
|-<br />
! C_CONTROL_CONFIG<br />
| 3 || Send new control parameters || 14 || [ KPnum1 KPden1 KDnum1 KDden1 KInum1 KIden1 Kslope1 KPnum2 KPden2 KDnum2 KDden2 KInum2 KIden2 Kslope2 ]<br />
|-<br />
! C_LOG_CONFIG<br />
| 4 || Send new logging parameters (unimplemented) || 0 || --<br />
|-<br />
! C_SET_PATH<br />
| 5 || Send new path points || variable || [ n time<sub>1</sub> theta1<sub>1</sub> theta2<sub>1</sub> time<sub>2</sub> theta1<sub>2</sub> theta2<sub>2</sub> time<sub>3</sub> theta1<sub>3</sub> theta2<sub>3</sub> ... time<sub>n</sub> theta1<sub>n</sub> theta2<sub>n</sub> ]<br />
|-<br />
! C_SEND_LOG<br />
| 6 || Asks the PIC to send logged data || 0 || -- <br />
|-<br />
! C_LOOP<br />
| 7 || Sets the looping option || 1 || 0 = run once, 1 = loop the path <br />
|-<br />
! C_STARTSTOP<br />
| 8 || start and stop (unimplemented) || 0 || -- <br />
|-<br />
! C_GOTO<br />
| 9 || Tells the arm to goto a certain X,Y || 2 || [ X Y ]<br />
|-<br />
! C_PAUSE<br />
| 10 || Stop and hold at current location || 0 || --<br />
|-<br />
|}<br />
<br />
For example, the vector that MATLAB sends to move the arm to X = 15, Y = -10 would be: [9 15 -10].<br />
<br />
===PIC C Code===<br />
[[Media:2dof-arm-v3-PIC.zip]]<br />
<br />
===MATLAB Code===<br />
[[Media:2dof-arm-v3-matlab.zip]]<br />
<br />
==Results==<br />
It was awesome.<br />
==Next Steps==<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.<br />
<br />
==References==</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=17909High Speed Motor Control2010-03-19T02:24:55Z<p>Sam Bobb: /* MATLAB Code */</p>
<hr />
<div>==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path.<br />
<br />
<br />
<math> x = L \cos (\theta_1)\ + \cos (\theta_1+\alpha)</math><br />
<br />
<math> y = L \sin(\theta_1)\ + \sin(\theta_1+\alpha)</math><br />
<br />
<math> \alpha = cos^{-1} \left(\frac{x^2+y^2-L^2-(2L)^2}{2L^2} \right)</math><br />
<br />
<math> \theta_1 = \frac{-(2 L \sin(\alpha)) x + (L + 2 L \cos(\alpha)) y} {(2 L \sin(\alpha)) y + (L + 2 L \cos(\alpha)) x}</math><br />
<br />
<math> \theta_2 = \theta_1 + \alpha \,</math><br />
<br />
<br />
Note: <math>\alpha\,</math> is used to calculate <math>\theta_{1,2}\,</math> in the MATLAB code and is not ever sent to the PIC.<br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
===Components===<br />
===Circuit Diagram===<br />
<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[insert screenshot of gui]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii [CHECK THIS WITH SAM}. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in COLOR is the path that the PIC tried to execute, and the COLOR plot shows the actual path of the arm, for comparison. INCLUDE SOMETHING ABOUT THE GET DATA AND CONTINUOUS LOG BUTTONS!<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. This table shows the available commands. <br />
{| class="wikitable" border="1"<br />
|+Commands<br />
|-<br />
! Command !! Identifying Integer !! Description !! Number of following ints !! Syntax for following ints<br />
|-<br />
! C_HOLD<br />
| 0 || Hold arm in home position || 2 || [ x_position y_position ]<br />
|-<br />
! C_FLOAT<br />
| 1 || Stop and float motors || 1 || [ true = 1 ]<br />
|-<br />
! C_RUN<br />
| 2 || Run path || 1 || [ true = 1 ]<br />
|-<br />
! C_CONTROL_CONFIG<br />
| 3 || Send new control parameters || 14 || [ KPnum1 KPden1 KDnum1 KDden1 KInum1 KIden1 Kslope1 KPnum2 KPden2 KDnum2 KDden2 KInum2 KIden2 Kslope2 ]<br />
|-<br />
! C_LOG_CONFIG<br />
| 4 || Send new logging parameters (unimplemented) || 0 || --<br />
|-<br />
! C_SET_PATH<br />
| 5 || Send new path points || variable || [ n time<sub>1</sub> theta1<sub>1</sub> theta2<sub>1</sub> time<sub>2</sub> theta1<sub>2</sub> theta2<sub>2</sub> time<sub>3</sub> theta1<sub>3</sub> theta2<sub>3</sub> ... time<sub>n</sub> theta1<sub>n</sub> theta2<sub>n</sub> ]<br />
|-<br />
! C_SEND_LOG<br />
| 6 || Asks the PIC to send logged data || 0 || -- <br />
|-<br />
! C_LOOP<br />
| 7 || Sets the looping option || 1 || 0 = run once, 1 = loop the path <br />
|-<br />
! C_STARTSTOP<br />
| 8 || start and stop (unimplemented) || 0 || -- <br />
|-<br />
! C_GOTO<br />
| 9 || Tells the arm to goto a certain X,Y || 2 || [ X Y ]<br />
|-<br />
! C_PAUSE<br />
| 10 || Stop and hold at current location || 0 || --<br />
|-<br />
|}<br />
<br />
===PIC C Code===<br />
[[Media:2dof-arm-v3-PIC.zip]]<br />
<br />
===MATLAB Code===<br />
[[Media:2dof-arm-v3-matlab.zip]]<br />
<br />
==Results==<br />
It was awesome.<br />
==Next Steps==<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.<br />
<br />
==References==</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=File:2dof-arm-v3-matlab.zip&diff=17908File:2dof-arm-v3-matlab.zip2010-03-19T02:24:27Z<p>Sam Bobb: High Speed Motion Control MATLAB code to interface with PIC 32 for control of a 2 DOF arm.</p>
<hr />
<div>High Speed Motion Control MATLAB code to interface with PIC 32 for control of a 2 DOF arm.</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=17907High Speed Motor Control2010-03-19T02:23:38Z<p>Sam Bobb: /* PIC C Code */</p>
<hr />
<div>==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path.<br />
<br />
<br />
<math> x = L \cos (\theta_1)\ + \cos (\theta_1+\alpha)</math><br />
<br />
<math> y = L \sin(\theta_1)\ + \sin(\theta_1+\alpha)</math><br />
<br />
<math> \alpha = cos^{-1} \left(\frac{x^2+y^2-L^2-(2L)^2}{2L^2} \right)</math><br />
<br />
<math> \theta_1 = \frac{-(2 L \sin(\alpha)) x + (L + 2 L \cos(\alpha)) y} {(2 L \sin(\alpha)) y + (L + 2 L \cos(\alpha)) x}</math><br />
<br />
<math> \theta_2 = \theta_1 + \alpha \,</math><br />
<br />
<br />
Note: <math>\alpha\,</math> is used to calculate <math>\theta_{1,2}\,</math> in the MATLAB code and is not ever sent to the PIC.<br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
===Components===<br />
===Circuit Diagram===<br />
<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[insert screenshot of gui]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii [CHECK THIS WITH SAM}. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in COLOR is the path that the PIC tried to execute, and the COLOR plot shows the actual path of the arm, for comparison. INCLUDE SOMETHING ABOUT THE GET DATA AND CONTINUOUS LOG BUTTONS!<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. This table shows the available commands. <br />
{| class="wikitable" border="1"<br />
|+Commands<br />
|-<br />
! Command !! Identifying Integer !! Description !! Number of following ints !! Syntax for following ints<br />
|-<br />
! C_HOLD<br />
| 0 || Hold arm in home position || 2 || [ x_position y_position ]<br />
|-<br />
! C_FLOAT<br />
| 1 || Stop and float motors || 1 || [ true = 1 ]<br />
|-<br />
! C_RUN<br />
| 2 || Run path || 1 || [ true = 1 ]<br />
|-<br />
! C_CONTROL_CONFIG<br />
| 3 || Send new control parameters || 14 || [ KPnum1 KPden1 KDnum1 KDden1 KInum1 KIden1 Kslope1 KPnum2 KPden2 KDnum2 KDden2 KInum2 KIden2 Kslope2 ]<br />
|-<br />
! C_LOG_CONFIG<br />
| 4 || Send new logging parameters (unimplemented) || 0 || --<br />
|-<br />
! C_SET_PATH<br />
| 5 || Send new path points || variable || [ n time<sub>1</sub> theta1<sub>1</sub> theta2<sub>1</sub> time<sub>2</sub> theta1<sub>2</sub> theta2<sub>2</sub> time<sub>3</sub> theta1<sub>3</sub> theta2<sub>3</sub> ... time<sub>n</sub> theta1<sub>n</sub> theta2<sub>n</sub> ]<br />
|-<br />
! C_SEND_LOG<br />
| 6 || Asks the PIC to send logged data || 0 || -- <br />
|-<br />
! C_LOOP<br />
| 7 || Sets the looping option || 1 || 0 = run once, 1 = loop the path <br />
|-<br />
! C_STARTSTOP<br />
| 8 || start and stop (unimplemented) || 0 || -- <br />
|-<br />
! C_GOTO<br />
| 9 || Tells the arm to goto a certain X,Y || 2 || [ X Y ]<br />
|-<br />
! C_PAUSE<br />
| 10 || Stop and hold at current location || 0 || --<br />
|-<br />
|}<br />
<br />
===PIC C Code===<br />
[[Media:2dof-arm-v3-PIC.zip]]<br />
<br />
===MATLAB Code===<br />
<br />
==Results==<br />
It was awesome.<br />
==Next Steps==<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.<br />
<br />
==References==</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=File:2dof-arm-v3-PIC.zip&diff=17906File:2dof-arm-v3-PIC.zip2010-03-19T02:22:49Z<p>Sam Bobb: High Speed Motion Control using the PIC 32 for control of a 2 DOF arm. Sam Bobb, 2010.</p>
<hr />
<div>High Speed Motion Control using the PIC 32 for control of a 2 DOF arm. Sam Bobb, 2010.</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=17905High Speed Motor Control2010-03-19T02:19:40Z<p>Sam Bobb: /* Overview */</p>
<hr />
<div>==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path.<br />
<br />
<br />
<math> x = L \cos (\theta_1)\ + \cos (\theta_1+\alpha)</math><br />
<br />
<math> y = L \sin(\theta_1)\ + \sin(\theta_1+\alpha)</math><br />
<br />
<math> \alpha = cos^{-1} \left(\frac{x^2+y^2-L^2-(2L)^2}{2L^2} \right)</math><br />
<br />
<math> \theta_1 = \frac{-(2 L \sin(\alpha)) x + (L + 2 L \cos(\alpha)) y} {(2 L \sin(\alpha)) y + (L + 2 L \cos(\alpha)) x}</math><br />
<br />
<math> \theta_2 = \theta_1 + \alpha \,</math><br />
<br />
<br />
Note: <math>\alpha\,</math> is used to calculate <math>\theta_{1,2}\,</math> in the MATLAB code and is not ever sent to the PIC.<br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
===Components===<br />
===Circuit Diagram===<br />
<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[insert screenshot of gui]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii [CHECK THIS WITH SAM}. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in COLOR is the path that the PIC tried to execute, and the COLOR plot shows the actual path of the arm, for comparison. INCLUDE SOMETHING ABOUT THE GET DATA AND CONTINUOUS LOG BUTTONS!<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. This table shows the available commands. <br />
{| class="wikitable" border="1"<br />
|+Commands<br />
|-<br />
! Command !! Identifying Integer !! Description !! Number of following ints !! Syntax for following ints<br />
|-<br />
! C_HOLD<br />
| 0 || Hold arm in home position || 2 || [ x_position y_position ]<br />
|-<br />
! C_FLOAT<br />
| 1 || Stop and float motors || 1 || [ true = 1 ]<br />
|-<br />
! C_RUN<br />
| 2 || Run path || 1 || [ true = 1 ]<br />
|-<br />
! C_CONTROL_CONFIG<br />
| 3 || Send new control parameters || 14 || [ KPnum1 KPden1 KDnum1 KDden1 KInum1 KIden1 Kslope1 KPnum2 KPden2 KDnum2 KDden2 KInum2 KIden2 Kslope2 ]<br />
|-<br />
! C_LOG_CONFIG<br />
| 4 || Send new logging parameters (unimplemented) || 0 || --<br />
|-<br />
! C_SET_PATH<br />
| 5 || Send new path points || variable || [ n time<sub>1</sub> theta1<sub>1</sub> theta2<sub>1</sub> time<sub>2</sub> theta1<sub>2</sub> theta2<sub>2</sub> time<sub>3</sub> theta1<sub>3</sub> theta2<sub>3</sub> ... time<sub>n</sub> theta1<sub>n</sub> theta2<sub>n</sub> ]<br />
|-<br />
! C_SEND_LOG<br />
| 6 || Asks the PIC to send logged data || 0 || -- <br />
|-<br />
! C_LOOP<br />
| 7 || Sets the looping option || 1 || 0 = run once, 1 = loop the path <br />
|-<br />
! C_STARTSTOP<br />
| 8 || start and stop (unimplemented) || 0 || -- <br />
|-<br />
! C_GOTO<br />
| 9 || Tells the arm to goto a certain X,Y || 2 || [ X Y ]<br />
|-<br />
! C_PAUSE<br />
| 10 || Stop and hold at current location || 0 || --<br />
|-<br />
|}<br />
<br />
===PIC C Code===<br />
===MATLAB Code===<br />
<br />
==Results==<br />
It was awesome.<br />
==Next Steps==<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.<br />
<br />
==References==</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=17904High Speed Motor Control2010-03-19T02:18:22Z<p>Sam Bobb: /* Overview */</p>
<hr />
<div>==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path.<br />
<br />
<br />
<math> x = L \cos (\theta_1)\ + \cos (\theta_1+\alpha)</math><br />
<br />
<math> y = L \sin(\theta_1)\ + \sin(\theta_1+\alpha)</math><br />
<br />
<math> \alpha = cos^{-1} \left(\frac{x^2+y^2-L^2-(2L)^2}{2L^2} \right)</math><br />
<br />
<math> \theta_1 = \frac{-(2 L \sin(\alpha)) x + (L + 2 L \cos(\alpha)) y} {(2 L \sin(\alpha)) y + (L + 2 L \cos(\alpha)) x}</math><br />
<br />
<math> \theta_2 = \theta_1 + \alpha \,</math><br />
<br />
<br />
Note: <math>\alpha\,</math> is used to calculate <math>\theta_{1,2}\,</math> in the MATLAB code and is not ever sent to the PIC.<br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
===Components===<br />
===Circuit Diagram===<br />
<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[insert screenshot of gui]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii [CHECK THIS WITH SAM}. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in COLOR is the path that the PIC tried to execute, and the COLOR plot shows the actual path of the arm, for comparison. INCLUDE SOMETHING ABOUT THE GET DATA AND CONTINUOUS LOG BUTTONS!<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. <br />
{| class="wikitable" border="1"<br />
|+Commands<br />
|-<br />
! Command !! Identifying Integer !! Description !! Number of following ints !! Syntax for following ints<br />
|-<br />
! C_HOLD<br />
| 0 || Hold arm in home position || 2 || [ x_position y_position ]<br />
|-<br />
! C_FLOAT<br />
| 1 || Stop and float motors || 1 || [ true = 1 ]<br />
|-<br />
! C_RUN<br />
| 2 || Run path || 1 || [ true = 1 ]<br />
|-<br />
! C_CONTROL_CONFIG<br />
| 3 || Send new control parameters || 14 || [ KPnum1 KPden1 KDnum1 KDden1 KInum1 KIden1 Kslope1 KPnum2 KPden2 KDnum2 KDden2 KInum2 KIden2 Kslope2 ]<br />
|-<br />
! C_LOG_CONFIG<br />
| 4 || Send new logging parameters (unimplemented) || 0 || --<br />
|-<br />
! C_SET_PATH<br />
| 5 || Send new path points || variable || [ n time<sub>1</sub> theta1<sub>1</sub> theta2<sub>1</sub> time<sub>2</sub> theta1<sub>2</sub> theta2<sub>2</sub> time<sub>3</sub> theta1<sub>3</sub> theta2<sub>3</sub> ... time<sub>n</sub> theta1<sub>n</sub> theta2<sub>n</sub> ]<br />
|-<br />
! C_SEND_LOG<br />
| 6 || Asks the PIC to send logged data || 0 || -- <br />
|-<br />
! C_LOOP<br />
| 7 || Sets the looping option || 1 || 0 = run once, 1 = loop the path <br />
|-<br />
! C_STARTSTOP<br />
| 8 || start and stop (unimplemented) || 0 || -- <br />
|-<br />
! C_GOTO<br />
| 9 || Tells the arm to goto a certain X,Y || 2 || [ X Y ]<br />
|-<br />
! C_PAUSE<br />
| 10 || Stop and hold at current location || 0 || --<br />
|-<br />
|}<br />
<br />
===PIC C Code===<br />
===MATLAB Code===<br />
<br />
==Results==<br />
It was awesome.<br />
==Next Steps==<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.<br />
<br />
==References==</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=17896High Speed Motor Control2010-03-19T01:57:27Z<p>Sam Bobb: /* Code */</p>
<hr />
<div>==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path.<br />
<br />
<br />
<math> x = L \cos (\theta_1)\ + \cos (\theta_1+\alpha)</math><br />
<br />
<math> y = L \sin(\theta_1)\ + \sin(\theta_1+\alpha)</math><br />
<br />
<math> \alpha = cos^{-1} \left(\frac{x^2+y^2-L^2-(2L)^2}{2L^2} \right)</math><br />
<br />
<math> \theta_1 = \frac{-(2 L \sin(\alpha)) x + (L + 2 L \cos(\alpha)) y} {(2 L \sin(\alpha)) y + (L + 2 L \cos(\alpha)) x}</math><br />
<br />
<math> \theta_2 = \theta_1 + \alpha \,</math><br />
<br />
<br />
Note: <math>\alpha\,</math> is used to calculate <math>\theta_{1,2}\,</math> in the MATLAB code and is not ever sent to the PIC.<br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
===Components===<br />
===Circuit Diagram===<br />
<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[insert screenshot of gui]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii [CHECK THIS WITH SAM}. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in COLOR is the path that the PIC tried to execute, and the COLOR plot shows the actual path of the arm, for comparison. INCLUDE SOMETHING ABOUT THE GET DATA AND CONTINUOUS LOG BUTTONS!<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
There are two main applications: the C code that runs on the PIC and the MATLAB code that runs on a PC. The two applications communicate by sending command packets over a serial connection. The packets consist of a string of 16-bit integers. The first integer identifies the command and the following integers carry the parameters. <br />
{| class="wikitable" border="1"<br />
|+Multiplication table<br />
|-<br />
! &times; !! 1 !! 2 !! 3<br />
|-<br />
! 1<br />
| 1 || 2 || 3<br />
|-<br />
! 2<br />
| 2 || 4 || 6<br />
|-<br />
! 3<br />
| 3 || 6 || 9<br />
|-<br />
! 4<br />
| 4 || 8 || 12<br />
|-<br />
! 5<br />
| 5 || 10 || 15<br />
|}<br />
<br />
===PIC C Code===<br />
===MATLAB Code===<br />
<br />
==Results==<br />
It was awesome.<br />
==Next Steps==<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.<br />
<br />
==References==</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=17849High Speed Motor Control2010-03-19T00:23:23Z<p>Sam Bobb: /* Overview */</p>
<hr />
<div>==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path.<br />
<br />
<br />
<math> x = L \cos (\theta_1)\ + \cos (\theta_1+\alpha)</math><br />
<br />
<math> y = L \sin(\theta_1)\ + \sin(\theta_1+\alpha)</math><br />
<br />
<math> \alpha = cos^{-1} \left(\frac{x^2+y^2-L^2-(2L)^2}{2L^2} \right)</math><br />
<br />
<math> \theta_1 = \frac{-(2 L \sin(\alpha)) x + (L + 2 L \cos(\alpha)) y} {(2 L \sin(\alpha)) y + (L + 2 L \cos(\alpha)) x}</math><br />
<br />
<math> \theta_2 = \theta_1 + \alpha \,</math><br />
<br />
<br />
Note: <math>\alpha\,</math> is used to calculate <math>\theta_{1,2}\,</math> in the MATLAB code and is not ever sent to the PIC.<br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
Encoders for position feedback.<br />
<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
===Components===<br />
===Circuit Diagram===<br />
<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[insert screenshot of gui]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii [CHECK THIS WITH SAM}. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in COLOR is the path that the PIC tried to execute, and the COLOR plot shows the actual path of the arm, for comparison. INCLUDE SOMETHING ABOUT THE GET DATA AND CONTINUOUS LOG BUTTONS!<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
===PIC C Code===<br />
===MATLAB Code===<br />
<br />
==Results==<br />
It was awesome.<br />
==Next Steps==<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.<br />
<br />
==References==</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=High_Speed_Motor_Control&diff=17848High Speed Motor Control2010-03-19T00:23:08Z<p>Sam Bobb: /* Overview */</p>
<hr />
<div>==Overview==<br />
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.<br />
<br />
===Team Members===<br />
[[Image:DanSamRyanTeamPic.jpg|thumb|300px|Sam, Ryan and Daniel with their robot.|right]]<br />
*Sam Bobb (Electrical Engineering senior, left)<br />
*Daniel Cornew (Mechanical Engineering junior, right)<br />
*Ryan Deeter (Mechanical Engineering junior, middle)<br />
<br />
<br />
<br clear=all><br />
<br />
==Mechanical Design==<br />
===Theory of Parallelogram Design===<br />
====Equations of Motion====<br />
Commanding the arm is much easier for a user to do in x- and y- coordinates than in motor angles or encoder counts. Therefore, equations were required that would translate x- and y- coordinates into angles from horizontal and then into encoder counts. Equations to express the reverse (encoder counts to angles to x- and y- coordinates) were also needed to evaluate the accuracy of the execution with respect to the command path.<br />
<br />
<br />
<math> x = L \cos (\theta_1)\ + \cos (\theta_1+\alpha)</math><br />
<br />
<math> y = L \sin(\theta_1)\ + \sin(\theta_1+\alpha)</math><br />
<br />
<math> \alpha = cos^{-1} \left(\frac{x^2+y^2-L^2-(2L)^2}{2L^2} \right)</math><br />
<br />
<math> \theta_1 = \frac{-(2 L \sin(\alpha)) x + (L + 2 L \cos(\alpha)) y} {(2 L \sin(\alpha)) y + (L + 2 L \cos(\alpha)) x}</math><br />
<br />
<math> \theta_2 = \theta_1 + \alpha \,</math><br />
<br />
<br />
Note: <math>\alpha\,</math> is used to calculate <math>\theta_{1,2}\,</math> in the MATLAB code and is not ever sent to the PIC.<br />
<br />
===Materials and Construction===<br />
The Motors are mounted into right-angle pieces of aluminum via screws in the face-plate of the motors. Each right angle has a slot milled into its base, and there is a flat aluminum base that also has a slot milled into it. The right angles are secured to this base with bolts, nuts, and lock washers. This slotted construction allows the position of the motors to be adjusted in order to ensure free movement of the arms. <br />
<br />
Attached to each motor is a carbon fiber arm. These arms are 1/2 inch thick carbon-nomex-carbon layups. (Nomex is a material that provides rigidity to carbon fiber.) Each arm has an aluminum block with a hole and set screw for mounting epoxied to one end. One of these carbon fiber arms has a bearing mounted in it 10 inches away from the motor shaft, and the other has a pin mounted at the same distance.<br />
<br />
There are two other component to the parallelogram assembly. One is a 22 inch by 1/2 inch length of carbon-nomex-carbon. This piece has one pin mounted 1 inch away from one end, and another pin mounted 10 inches away from that. The other piece is a 12 inch long piece of aluminum that has been bent into a 1 inch by in inch U-shape. This piece has two ball-bearing epoxied into it 10 inches apart.<br />
<br />
The longer piece of carbon has the pin closest to its end press fit into the ball bearing mounted into the motor arm. The U-shaped piece of aluminum has on ball bearing slid onto the other motor arm, and secured with a snap ring. The remaining ball bearing is then slid onto the reaming pin in the long piece of carbon and secured with a snap ring.<br />
<br />
The position of the motors relative to one another in then adjusted unit the arms move freely, and then fastened in place.<br />
<br />
==Electrical Design==<br />
===Overview===<br />
Encoders for position feedback<br />
Optical break sensor on each arm to establish absolute position<br />
<br />
===Components===<br />
===Circuit Diagram===<br />
<br />
<br />
==GUI==<br />
<br />
The GUI was programmed in MATLAB using the "guide" function. The GUI code calls the other MATLAB functions and is rather small as far as the amount of new code in contains.<br />
<br />
[insert screenshot of gui]<br />
<br />
===Usage===<br />
<br />
The GUI is made up of four main sections that allow a user to control the path and motor control parameters of the arm and plot the results.<br />
<br />
The first section is the "Path" section, which allows users to specify the type of path the arm will follow. There are currently three path choices: "go to," "circle," and "trace." The "go to" path allows the user to specify a point that the arm will go to from its current position in five seconds. The "circle" path is determined through five values. The first two are the (x, y) of the center of the circle. The next two are coefficients on sin and cosine terms that are effectively the radii [CHECK THIS WITH SAM}. The last value is the time in seconds for the arm to complete the circle. In the final path, "trace," users input four (x, y) positions and the arm moves from the first to the fourth and back to the first with two seconds in between points. The GUI could easily be changed so that the user specifies (x, y, t) and controls the time between points, but this would require velocity-based failure checking in the code to ensure that the user did not give a command that was dangerous to the arm.<br />
<br />
The section just to the right of the "Path" section allows for further manipulation of the path and motor control. A checkbox controls whether or not the specified path loops (note: "go to" cannot loop). Beneath the checkbox is a section labeled "PID Control" and it lets users adjust the coefficients kp (proportional), ki (integral) and kd (derivative) on the fly by pressing the "Update" button. <br />
<br />
The section on the far right is used to communicate between MATLAB and the PIC. A COM port is specified for communication. The "Connect" button opens the COM port, and the "Disconnect" button closes it. The "Start" button runs whichever path is specified in the "Path" section, and "Pause" pauses the path (note: to unpause, click the "Start" button again while paused).<br />
<br />
The final section of the GUI is the "Graphs" section. This section plots the path of the arm in either "theta vs t" or "x vs y," as selected by the radio buttons. Plotted in COLOR is the path that the PIC tried to execute, and the COLOR plot shows the actual path of the arm, for comparison. INCLUDE SOMETHING ABOUT THE GET DATA AND CONTINUOUS LOG BUTTONS!<br />
<br />
===Code===<br />
Using MATLAB's "guide" function when creating GUIs makes for rather straightforward coding. When a component is added to the GUI (be it a button, text field, etc.), MATLAB adds a block of code to an automatically generated m-file. These blocks are functions, which means that programming them requires the use of handles. Here is an example:<br />
<br />
function xCoor_Callback(hObject, eventdata, handles)<br />
% hObject handle to xCoor (see GCBO)<br />
% eventdata reserved - to be defined in a future version of MATLAB<br />
% handles structure with handles and user data (see GUIDATA)<br />
if get(handles.goToButton, 'Value') == 1<br />
xCoor = get(hObject, 'String');<br />
end<br />
<br />
<br />
This block of code is for the x-coordinate of the "Go To" path option. The if statement utilizes handles so that the "Go To" button's function can be accessed from the x-coordinate function. This block sets the x-coordinate of the "Go To" command to the user specified value in the GUI if the "Go To" button is selected.<br />
<br />
==Code==<br />
===Overview===<br />
===PIC C Code===<br />
===MATLAB Code===<br />
<br />
==Results==<br />
It was awesome.<br />
==Next Steps==<br />
<br />
==Acknowledgements==<br />
We would like to acknowledge Professor Lynch, Nick Marchuk and Andy Long for their instruction and guidance throughout this project.<br />
<br />
==References==</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=PIC32MX:_FFT_of_Analog_Input&diff=16316PIC32MX: FFT of Analog Input2010-02-16T18:49:17Z<p>Sam Bobb: /* Example Plots */</p>
<hr />
<div>== Original Assignment ==<br />
<br />
'''Do not erase this section!'''<br />
<br />
Your assignment is to read in several cycles of a periodic signal from a function generator (e.g., a sine wave or a square wave), perform an FFT on your samples using your PIC32, and send back the results to a PC to display. Try this for 256 samples and 1024 samples, and determine how long it takes to compute the FFT for each, once you have collected the data. Verify that the results make sense according to the input signals you've used; for example, a sine wave should have all power at the chosen frequency, while a square wave will have much of the power at the frequency of the wave, but power at other frequencies, too.) Check out the MPLAB C32 Libraries manual to learn more about the dsplib_dsp.h, which you must include. Try both mips_fft16 and mips_fft32.<br />
<br />
== Overview ==<br />
A fast Fourier transform (FFT) is a method to calculate a discrete Fourier transform (DFT). More information about [http://en.wikipedia.org/wiki/Fast_Fourier_transform FFTs] and [http://en.wikipedia.org/wiki/Discrete_Fourier_transform DFTs] can be found on wikipedia (linked).<br />
The following circuit and code allow a user to put a signal into a PIC32, perform an FFT on that signal, output the data to Matlab via RS-232, and view a plot showing the raw signal, the FFT as calculated by the PIC, and the FFT as calculated by Matlab. Viewing the Matlab calculation is for verification only and can be commented out of the Matlab code.<br />
<br />
<br />
Commented C code and Matlab code are provided at the bottom of this page and can be downloaded [[Media: Lab5FFTcode.zip | here]].<br />
<br />
== Circuit ==<br />
<br />
Beyond simple inputs and outputs, there is no special circuitry required for computing FFTs. The signal is inserted into pin B4 on the PIC and must be below VREF on the PIC (currently configured to 3.3 V). For data output, the PIC uses RS-232 communication. Three connections need to be made with the RS-232:<br />
<br />
*Orange RS-232 wire to pin F4 on the PIC<br />
*Yellow RS-232 wire to pin F5 on the PIC<br />
*Black RS-232 wire to ground<br />
<br />
The remaining connection is not essential for FFT caclulation but allows the user to monitor the calculation time. Pin A14 goes high during calculation and goes back low when the calculation is completed. This feature lets the calculation time be monitored on an oscilloscope.<br />
<br />
The following circuit diagram illustrates the signal generator to PIC connection, the RS-232 connections, and the optional oscilloscope connection.<br />
<br />
<br />
<br />
[[Image:FftSchematic.png]]<br />
<br />
== Code ==<br />
The following is the source code to be programmed onto the PIC. If unfamiliar, see [[HelloWorld_PIC32 | here]] for a guide to creating a .hex file with MPLAB and [[Directions_to_Load_Files_to_PIC32_with_HID_Bootloader | here]] for directions to load files to the PIC. It first establishes inputs, outputs and RS-232 communication. When prompted with a 'p' (as written, it is prompted automatically by Matlab), the PIC computes the FFT then sends the magnitude portion of the single-sided FFT and the raw samples to Matlab.<br />
<br />
<pre><br />
/* Sam Bobb, Daniel Cornew, Ryan Deeter<br />
Fast Fourier Transform (FFT) Example<br />
ME 333, Lab 5<br />
Feb 2010<br />
*/<br />
<br />
/** INCLUDES ***************************************************/<br />
#include "HardwareProfile.h"<br />
#include <plib.h><br />
#include "dsplib_dsp.h"<br />
#include "fftc.h"<br />
<br />
<br />
/** Constants **************************************************/<br />
<br />
#define TRUE 1<br />
#define FALSE 0<br />
<br />
#define LOOP_TIME_PIN LATAbits.LATA14<br />
<br />
#define DESIRED_BAUDRATE (19200) // The desired BaudRate <br />
<br />
// To modify the number of samples:<br />
#define fftc fft16c256 //from fftc.h, for N = 256 use fft16c256, for N = 1024 use fft16c1024 <br />
#define N 256 // Also change the log2N variable below!!<br />
<br />
// To modify the sampling frequency<br />
#define SAMPLE_FREQ 10000<br />
// Also change the timer interupt setup in initInterruptController!<br />
<br />
/** Function Declarations **************************************/<br />
void initInterruptController();<br />
<br />
void initUART2(int pbClk);<br />
<br />
void sendDataRS232(void);<br />
<br />
void computeFFT(void); //sets up function to compute FFT<br />
<br />
<br />
<br />
<br />
<br />
/** Global Variables *******************************************/<br />
<br />
int computeFFTflag = FALSE; //used to indicate that an fft computation has been requested over rs232<br />
<br />
int sampleIndex = 0; //keeps track of where we're putting our ADC reading<br />
<br />
/** establish variables for FFT calculation*/<br />
// -----<br />
// this has to be changed for different values of N<br />
int log2N = 8; // log2(256) = 8<br />
//int log2N = 10; // log2(1024) = 10<br />
// -----<br />
<br />
int16c sampleBuffer[N]; //initialize buffer to collect samples<br />
int16c calcBuffer[N]; // initialize buffer to hold old samples<br />
int16c scratch[N];<br />
int16c dout[N]; //holds computed FFT until transmission<br />
<br />
long int singleSidedFFT[N];<br />
long int freqVector[N];<br />
<br />
/** Main Function **********************************************/<br />
<br />
int main(void)<br />
{<br />
int pbClk;<br />
int i;<br />
<br />
// Configure the proper PB frequency and the number of wait states<br />
pbClk = SYSTEMConfigPerformance(SYS_FREQ);<br />
<br />
TRISAbits.TRISA14 = 0;<br />
<br />
// Allow vector interrupts<br />
INTEnableSystemMultiVectoredInt();<br />
<br />
mInitAllLEDs();<br />
<br />
initInterruptController();<br />
<br />
initUART2(pbClk);<br />
<br />
// Configure the proper PB frequency and the number of wait states<br />
SYSTEMConfigPerformance(SYS_FREQ);<br />
<br />
// -------- Set Up ADC --------<br />
// configure and enable the ADC<br />
CloseADC10(); // ensure the ADC is off before setting the configuration<br />
<br />
// define setup parameters for OpenADC10<br />
// Turn module on | output in integer | trigger mode auto | enable autosample<br />
#define PARAM1 ADC_MODULE_ON | ADC_FORMAT_INTG16 | ADC_CLK_AUTO | ADC_AUTO_SAMPLING_ON<br />
<br />
// define setup parameters for OpenADC10<br />
// ADC ref external | disable offset test | enable scan mode | perform 2 samples | use one buffer | use MUXA mode<br />
#define PARAM2 ADC_VREF_AVDD_AVSS | ADC_OFFSET_CAL_DISABLE | ADC_SCAN_ON | ADC_SAMPLES_PER_INT_2 | ADC_ALT_BUF_OFF | ADC_ALT_INPUT_OFF<br />
<br />
// define setup parameters for OpenADC10<br />
// use ADC internal clock | set sample time<br />
#define PARAM3 ADC_CONV_CLK_INTERNAL_RC | ADC_SAMPLE_TIME_15<br />
<br />
// define setup parameters for OpenADC10<br />
// set AN4<br />
#define PARAM4 ENABLE_AN4_ANA<br />
<br />
// define setup parameters for OpenADC10<br />
// do not assign channels to scan<br />
#define PARAM5 SKIP_SCAN_AN0 | SKIP_SCAN_AN1 | SKIP_SCAN_AN2 | SKIP_SCAN_AN3 | SKIP_SCAN_AN5 | SKIP_SCAN_AN6 | SKIP_SCAN_AN7 | SKIP_SCAN_AN8 | SKIP_SCAN_AN9 | SKIP_SCAN_AN10 | SKIP_SCAN_AN11 | SKIP_SCAN_AN12 | SKIP_SCAN_AN13 | SKIP_SCAN_AN14 | SKIP_SCAN_AN15<br />
<br />
// use ground as neg ref for A | use AN4 (B4) for input A<br />
// configure to sample AN4<br />
SetChanADC10( ADC_CH0_NEG_SAMPLEA_NVREF); // configure to sample AN4<br />
OpenADC10( PARAM1, PARAM2, PARAM3, PARAM4, PARAM5 ); // configure ADC using parameter define above<br />
<br />
EnableADC10(); // Enable the ADC<br />
<br />
while ( ! mAD1GetIntFlag() ) { } // wait for the first conversion to complete so there will be valid data in ADC result registers<br />
// ------ DONE SETTING UP ADC ------<br />
<br />
// zero the freqVector and singleSidedFFT<br />
for (i=0; i<N; i++)<br />
{<br />
freqVector[i] = 0;<br />
singleSidedFFT[i] = 0;<br />
}<br />
<br />
// generate frequency vector<br />
// this is the x-axis of your single sided fft<br />
for (i=0; i<N/2; i++)<br />
{<br />
freqVector[i] = i*(SAMPLE_FREQ/2)/((N/2) - 1);<br />
}<br />
<br />
// wait around until you get an fft command from rs232<br />
// then call computeFFT to generate an fft of the last block of samples<br />
// then send out rs232.<br />
// If you want to use FFT without rs232, you can call computeFFT when ever you want in your program.<br />
while(1)<br />
{<br />
if (computeFFTflag == TRUE)<br />
{<br />
computeFFT();<br />
sendDataRS232();<br />
} <br />
}<br />
<br />
CloseOC1();<br />
<br />
<br />
} //end main<br />
<br />
<br />
/** Interrupt Handlers *****************************************/<br />
// interrput code for the timer 3<br />
void __ISR( _TIMER_3_VECTOR, ipl7) T3Interrupt( void)<br />
{<br />
int i;<br />
//LOOP_TIME_PIN = TRUE;<br />
// When we used the loop time pin to measure the length of this ISR,<br />
// we measured 400 ns, so you could sample at over 1 MHz.<br />
<br />
sampleBuffer[sampleIndex].re = ReadADC10(0); // read the ADC into the real part of the samplebuffer<br />
sampleBuffer[sampleIndex].im = 0; // the imaginary value is 0.<br />
<br />
// you could shave a little time off this ISR by just zeroing the .im value once, outside the ISR<br />
<br />
// increment the sampleIndex<br />
if (sampleIndex == (N-1))<br />
{<br />
sampleIndex = 0;<br />
}<br />
else<br />
{<br />
sampleIndex++;<br />
} <br />
<br />
<br />
//LOOP_TIME_PIN = FALSE;<br />
<br />
// clear interrupt flag and exit<br />
mT3ClearIntFlag();<br />
} // T3 Interrupt<br />
<br />
<br />
// UART 2 interrupt handler<br />
// it is set at priority level 2<br />
void __ISR(_UART2_VECTOR, ipl2) IntUart2Handler(void)<br />
{<br />
char data;<br />
<br />
// Is this an RX interrupt?<br />
if(mU2RXGetIntFlag())<br />
{<br />
// Clear the RX interrupt Flag<br />
mU2RXClearIntFlag();<br />
<br />
data = ReadUART2();<br />
// Echo what we just received.<br />
putcUART2(data);<br />
<br />
switch(data)<br />
{<br />
<br />
case 'p': // compute and output the FFT<br />
computeFFTflag = TRUE;<br />
break; <br />
} <br />
<br />
// Toggle LED to indicate UART activity<br />
mLED_0_Toggle();<br />
<br />
}<br />
<br />
// We don't care about TX interrupt<br />
if ( mU2TXGetIntFlag() )<br />
{<br />
mU2TXClearIntFlag();<br />
}<br />
}<br />
<br />
<br />
<br />
<br />
/** Other Functions ********************************************/<br />
<br />
void initInterruptController(void)<br />
{<br />
// init Timer3 mode and period (PR3) <br />
OpenTimer3( T3_ON | T3_PS_1_1 | T3_SOURCE_INT, 0x1F40); // produces 100 ms period<br />
// sampling frequency = 10 kHz<br />
<br />
mT3SetIntPriority( 7); // set Timer3 Interrupt Priority<br />
mT3ClearIntFlag(); // clear interrupt flag<br />
mT3IntEnable( 1); // enable timer3 interrupts<br />
}<br />
<br />
void initUART2(int pbClk)<br />
{<br />
// define setup Configuration 1 for OpenUARTx<br />
// Module Enable <br />
// Work in IDLE mode <br />
// Communication through usual pins <br />
// Disable wake-up <br />
// Loop back disabled <br />
// Input to Capture module from ICx pin <br />
// no parity 8 bit <br />
// 1 stop bit <br />
// IRDA encoder and decoder disabled <br />
// CTS and RTS pins are disabled <br />
// UxRX idle state is '1' <br />
// 16x baud clock - normal speed<br />
#define config1 UART_EN | UART_IDLE_CON | UART_RX_TX | UART_DIS_WAKE | UART_DIS_LOOPBACK | UART_DIS_ABAUD | UART_NO_PAR_8BIT | UART_1STOPBIT | UART_IRDA_DIS | UART_DIS_BCLK_CTS_RTS| UART_NORMAL_RX | UART_BRGH_SIXTEEN<br />
<br />
// define setup Configuration 2 for OpenUARTx<br />
// IrDA encoded UxTX idle state is '0'<br />
// Enable UxRX pin<br />
// Enable UxTX pin<br />
// Interrupt on transfer of every character to TSR <br />
// Interrupt on every char received<br />
// Disable 9-bit address detect<br />
// Rx Buffer Over run status bit clear<br />
#define config2 UART_TX_PIN_LOW | UART_RX_ENABLE | UART_TX_ENABLE | UART_INT_TX | UART_INT_RX_CHAR | UART_ADR_DETECT_DIS | UART_RX_OVERRUN_CLEAR <br />
<br />
// Open UART2 with config1 and config2<br />
OpenUART2( config1, config2, pbClk/16/DESIRED_BAUDRATE-1); // calculate actual BAUD generate value.<br />
<br />
// Configure UART2 RX Interrupt with priority 2<br />
ConfigIntUART2(UART_INT_PR2 | UART_RX_INT_EN);<br />
}<br />
<br />
<br />
void sendDataRS232()<br />
{<br />
int i;<br />
char RS232_Out_Buffer[32]; // max characters per line (line feed and carriage return count)<br />
<br />
sprintf(RS232_Out_Buffer,"\n\rSTART\n\r"); //print START. MATLAB uses this as a start delimiter<br />
putsUART2(RS232_Out_Buffer);<br />
<br />
sprintf(RS232_Out_Buffer,"ROWS=%d\n\r", N); //print the number of rows, so matlab can make the correct sized buffer<br />
putsUART2(RS232_Out_Buffer);<br />
<br />
for(i = 0; i < N; i++)<br />
{<br />
// output: frequency vector, the calculated fft, raw samples<br />
sprintf(RS232_Out_Buffer,"%d %d %d\n\r",freqVector[i], singleSidedFFT[i], calcBuffer[i].re); <br />
putsUART2(RS232_Out_Buffer);<br />
}<br />
<br />
sprintf(RS232_Out_Buffer,"END\n\r"); //output end so matlab knows we're done<br />
putsUART2(RS232_Out_Buffer);<br />
<br />
}<br />
<br />
<br />
void computeFFT()<br />
{<br />
// when using 256 samples, we measured this function to take about 500 microseconds<br />
// (not including the time to send rs232 data)<br />
int i;<br />
<br />
mT3IntEnable(0); //turns off interrupt while computing FFT<br />
<br />
LOOP_TIME_PIN = TRUE;<br />
<br />
for (i=0; i<N; i++)<br />
{<br />
if (i<sampleIndex)<br />
{<br />
// old chunk<br />
calcBuffer[i+(N-sampleIndex)] = sampleBuffer[i];<br />
} <br />
else // i >= sampleIndex<br />
{<br />
// new chunk<br />
calcBuffer[i-sampleIndex] = sampleBuffer[i];<br />
} <br />
<br />
} <br />
<br />
// load complex input data into din<br />
mips_fft16(dout, calcBuffer, fftc, scratch, log2N);<br />
<br />
// compute single sided fft<br />
for(i = 0; i < N/2; i++)<br />
{<br />
singleSidedFFT[i] = 2 * ((dout[i].re*dout[i].re) + (dout[i].im*dout[i].im));<br />
}<br />
<br />
LOOP_TIME_PIN = FALSE;<br />
<br />
computeFFTflag = FALSE;<br />
<br />
// do something with dout<br />
mT3IntEnable(1); //turn interrupt back on<br />
}<br />
<br />
</pre><br />
<br />
The following Matlab code computes its own FFT with the raw samples. It then plots the input signal, the PIC calculated FFT, and the Matlab calculated FFT, for comparison.<br />
<br />
<pre><br />
<br />
% fftserial interface v 2<br />
<br />
%Sam Bobb, Daniel Cornew, Ryan Deeter<br />
%ME 333 Lab 5: FFT of Analog Input, Winter 2010<br />
<br />
%This code receives the single sided magnitude FFT calculated on the PIC<br />
%and the raw samples through serial communication. Matlab calculates its<br />
%own FFT of the raw samples, then plots the input signal, PIC calculated<br />
%FFT, and Matlab calculated FFT.<br />
<br />
<br />
<br />
<br />
Fs = 10000; % Sampling frequency<br />
T = 1/Fs; % Sample time<br />
L = 256; % Length of signal<br />
t = (0:L-1)*T; % Time vector<br />
<br />
<br />
%for runtimes=1:100 %UNCOMMENT FOR CONTINUOUS FFT<br />
<br />
columns = 3;<br />
<br />
%check if the serial port already exists<br />
%if it does, close it, it probably means something bad happened last run<br />
if exist('COM','var')<br />
disp('closing old port instance');<br />
fclose(COM)<br />
delete(COM)<br />
clear COM<br />
disp('port closed');<br />
end<br />
<br />
% open the serial port<br />
COM = serial('COM6');<br />
set(COM,'BaudRate',19200);<br />
fopen(COM) <br />
<br />
disp('port opened');<br />
<br />
% send the command to print data<br />
fprintf(COM,'p')<br />
<br />
<br />
%there's usually some garbage at the beginning (like the echoed p <br />
%character command and line breaks); this reads lines in until we find <br />
%the start line<br />
text = fscanf(COM, '%s');<br />
while isempty(strfind(text, 'START'))<br />
text = fscanf(COM, '%s');<br />
end<br />
<br />
<br />
% read number of rows<br />
rowstext = fscanf(COM, '%s');<br />
pat = 'ROWS=';<br />
split = regexp(rowstext, pat, 'split');<br />
rows = str2double(split(2));<br />
<br />
DATA = zeros(rows,columns);<br />
<br />
%generate the fscanf format argument<br />
%repeate the formatchar character for the number of columns<br />
formatchar = '%d';<br />
serialpat = '';<br />
<br />
%establishing fscanf format<br />
for j = 1:columns<br />
serialpat = [serialpat ' ' formatchar];<br />
end<br />
<br />
%reads serial data and puts it into DATA<br />
for j = 1:rows<br />
DATA(j, :) = fscanf(COM, serialpat, [columns 1]);<br />
%disp(COM.BytesAvailable)<br />
<br />
end<br />
<br />
%reads last line<br />
last = fscanf(COM, '%s');<br />
<br />
%verifies that last line read is the last line<br />
if strcmp(last,'END')<br />
disp('Success')<br />
else<br />
disp('Problem')<br />
end<br />
<br />
%disp(COM.BytesAvailable) %for debugging<br />
<br />
%plotting input signal<br />
subplot(3, 1, 1)<br />
plot(DATA(:,3))<br />
title('Time Domain Signal')<br />
<br />
%plotting PIC calculated FFT<br />
subplot(3, 1, 2)<br />
plot(DATA(:,1), DATA(:,2))<br />
title('Single Sided FFT Calculated on the PIC')<br />
xlabel('Frequency (Hz)')<br />
ylabel('|Y(f)|')<br />
axis([0 Fs/2 0 2e5])<br />
<br />
%calculating FFT in Matlab<br />
NFFT = 2^nextpow2(L); % Next power of 2 from length of y<br />
Y = fft(DATA(:,3),NFFT)/L;<br />
<br />
%Y = DATA(:,1) + DATA(:,2)*1i;<br />
<br />
f = Fs/2*linspace(0,1,NFFT/2+1);<br />
<br />
% Plot Matlab's single-sided amplitude spectrum.<br />
subplot(3, 1, 3)<br />
plot(f,2*abs(Y(1:NFFT/2+1)))<br />
title('Single Sided FFT Calculated with MATLAB')<br />
xlabel('Frequency (Hz)')<br />
ylabel('|Y(f)|')<br />
<br />
%pause(2) %UNCOMMENT FOR CONTINOUS FFT<br />
<br />
%end %UNCOMMENT FOR CONTINUOUS FFT<br />
<br />
%closes serial port<br />
fclose(COM)<br />
delete(COM)<br />
clear COM<br />
disp('port closed');<br />
<br />
</pre><br />
<br />
==Example Plots==<br />
Here is an example using a 500 Hz sine wave input.<br />
[[Image: FFT_500_sin.jpg|border|700px]]<br />
<br />
This example uses a 1 kHz sine wave.<br />
[[Image: FFT_1K_sin.jpg|border|700px]]<br />
<br />
This example is still at 1 kHz, but with a square wave.<br />
[[Image: FFT_1K_square.jpg|border|700px]]<br />
<br />
This example shows a 2 kHz sine wave.<br />
[[Image: FFT_2K_sin.jpg|border|700px]]<br />
<br />
This example, still at 2 kHz, is a square wave.<br />
[[Image: FFT_2K_square.jpg|border|700px]]<br />
<br />
The final example is a 3 kHz sine wave.<br />
[[Image: FFT_3K_sin.jpg|border|700px]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=PCB_Artist&diff=15118PCB Artist2010-01-13T19:56:28Z<p>Sam Bobb: /* Download and Installation Instructions */</p>
<hr />
<div>[[image:CircuitBoard.jpg|400px|right]]<br />
<br />
== What is a PCB? ==<br />
A printed circuit board, or PCB, is an epoxy board clad with copper sheets to make electrical connections along fixed routes. A PCB can have many layers of copper and insulator, depending on the complexity of the design. Layers are connected with vias, plated holes maintaining electrical conductivity. A finished PCB is often covered with solder mask, a solder-phobic layer that helps prevent solder bridges, making the PCB green. A white silk screen can be drawn on top of this, used to label the outlines of components and give other critical information. For more information on PCBs, see the wikipedia page [http://en.wikipedia.org/wiki/Printed_circuit_board Printed Circuit Board].<br />
<br />
PCBs are created in a variety of ways. A common method is covering the copper you wish to keep and dipping the board in acid, which etches away the copper you do not want. This method can be done by hand. <br />
<br />
A more professional and reliable PCB can be made by designing your circuit in CAD software and ordering the part from a PCB production house. The remainder of this page will describe how to use the proprietary software PCB Artist to design and order a complete PCB.<br />
<br />
== Advanced Circuits and PCB Artist ==<br />
<br />
Advanced Circuits [http://www.4pcb.com (www.4pcb.com)] is a company based in Colorado known for their quick turn-around and good prices on low volume printed circuit board (PCB) orders. This is the main supplier for PCBs created by students for clases or research at Northwestern.<br />
<br />
Previously, the PCB was created in TraxxMaker or other CAD PCB program, many of which are outdated and difficult to use, then uploaded to Advanced Circuits. Advanced Circuits has created PCB Artist, free software to create schematics and PCBs. This software is designed to order only from Advanced Circuits. This somewhat simplistic software is very intuitive (once comfortable) and allows for simple changes and editing. Additionally, the component library offered is up to date and contains many specific components. This Wiki page will cover the development and ordering process of a simple PCB board. <br />
<br />
====Contact at Advanced Circuits====<br />
<br />
Jackie Sartin - Regional Sales Manager<br />
<br />
E-mail: jackies@4pcb.com<br />
Call: 1-800-979-4PCB (4722) Ext: 1685<br />
<br />
== Getting Set Up ==<br />
*Have a complete circuit schematic and know exactly what parts (including part numbers) you are using.<br />
<br />
*[http://www.4pcb.com/index.php?load=content&page_id=46 Download the software] Or, type “PCB Artist” into google. Then click the download button in the middle of the screen.<br />
<br />
*Review the material on this page: [http://www.4pcb.com/index.php?load=content&page_id=306 Help Guides and User Reference documents]<br />
<br />
Browse through the [http://www.4pcb.com/downloads/pcbartist/PCBArtistTutorial.pdf Advanced Circuits tutorial pdf] to get familiar with the program interface and design process. In general, you will:<br />
<br />
*Create a schematic (.sch) that represents the circuit you want to create. This schematic is just like any other schematic you would draw by hand, but it will be used to automate the process of laying out your physical PCB and is very helpful in debugging.<br />
<br />
*Create a PCB design (.pcb) that represents your physical PCB, based on your schematic. In this CAD process, the program will automatically grab the components you need and place them outside of the board. You then place each component and connect the pins based on how they are connected in the schematic. The program helps you by highlighting the nets (all the pins that need to be connected) in different colors. You choose how to connect each pin, the width of the copper trace, the layer the copper should be on (top or bottom for a 2 sided board), and the size of the vias. You can have PCB Artist automatically place your components and connect them, but the result may not be pretty or successful. <br />
<br />
*Check for errors. Use the Design Rule Check tool to make sure the board is machinable and the pins are connected as they are in your schematic. Carefully scan your board to make sure vias don't accidentally touch traces. Put some silk screen labels next to pins and components, and put your name and the version of your board somewhere for later reference.<br />
<br />
*Order your board! Use the student deal ($33 for one board), but note it generally takes a week to make and a few days to ship (another $15). So order early and design carefully.<br />
<br />
Some general tips:<br />
<br />
*Don't try to make a 2" x 2" board if your space constraints allow for a larger board. It is not worth the effort and possible mistakes trying to squeeze everything into a tiny area if you don't have to.<br />
<br />
*Connect your power and ground traces first, they are usually the most complicated.<br />
<br />
*Try to keep an organized board. Keep components with many connections close together.<br />
<br />
*Use a convention like top layer traces go left-right and bottom layer traces go up-down, this will make routing much easier.<br />
<br />
*Give yourself some prototyping area and add extra vias to traces. Thus will allow you some wiggle room when your board arrives and has errors (every board will have some errors!). Traces can be cut with a razor blade fairly easily if the board is not too tight, and can be remade with wires if you remember to leave some extra vias around to solder to.<br />
<br />
==Download and Installation Instructions==<br />
<br />
Download and install PCB Artist from Advanced Circuits at [http://www.4pcb.com/index.php?load=content&page_id=46 the Advanced Artist website.] The latest version as of 1/7/2010 is 1.3.2. It is PC only and works best in Windows XP.<br />
<br />
To install a library, download [[Media:ME333_Lab22.zip | the example .zip file here]]. Extract the files and place them in the PCB Artist Library folder. On Windows XP the default location is: C:\Documents and Settings\All Users\Documents\PCB Artist\Library, and on Windows Vista or Windows 7 the default location is C:\Users\Public\Documents\PCB Artist\Library.<br />
<br />
[[Image:library.jpg]]<br />
<br />
==Create a Schematic==<br />
Open a new schematic.<br />
<br />
[[Image:new_schematic.jpg]]<br />
<br />
Select Add a Component.<br />
<br />
[[Image:select_component.jpg]]<br />
<br />
Choose the library your component is in (the ME333 library).<br />
<br />
[[Image:select_schematic_library.jpg]]<br />
<br />
Place the component R-3. Components should be arranged so that the schematic can be read and understood easily, they do not need to be laid out as they would on the PCB, we will do that later. To drag components around in the schematic, click the arrow button in the toolbar at the top to get the arrow cursor.<br />
<br />
[[Image:place_schematic_component.jpg]]<br />
<br />
Add the LED from the ME333 library and connect the components using the Add Schematic Connection tool (the icon on the left that looks like a pencil drawing a line). Such connections will make it easier to create the PCB. They indicate which pins need to be connected on the PCB, but they do not represent the actual paths the traces will take. We will design those later.<br />
<br />
[[Image:connect_schematic_components.jpg]]<br />
<br />
Add a voltage source and ground from the Schema library and connect them to the schematic.<br />
<br />
[[Image:complete_schematic.jpg]]<br />
<br />
Save the file (.sch). Now we can design a PCB for this circuit.<br />
<br />
==Create PCB from Schematic==<br />
Open your schematic and go to Tools, Schematic <--> PCB, Translate to PCB.<br />
<br />
[[Image:translate_to_pcb.jpg]]<br />
<br />
Go through the New PCB Wizard, selecting your board size or defining a custom size. This can always be changed later if you need a larger or smaller board.<br />
<br />
[[Image:board_size.jpg]]<br />
<br />
Use a 2 layer board with the default properties. At this screen,<br />
<br />
[[Image:special_reqs.jpg]]<br />
<br />
do not enter anything, but later we will add a comment to allow us to buy a Student discount board for $33 plus shipping.<br />
<br />
Enter a board number and revision number. Use a 1 week turn around with 1 unit. Ignore the cost, that will change to $33 later.<br />
<br />
[[Image:pcb_production.jpg]]<br />
<br />
Have the program place the components outside of your board so you can arrange them where you want them.<br />
<br />
[[Image:place_components.jpg]]<br />
<br />
Translate the components to the inside of the green box which represents the outline of your board. Rotate a component by right clicking on the part and choosing rotate. All of the components must be moved inside of the green box to finish the board. These will be their locations on the board.<br />
<br />
[[Image:move_and_rotate.jpg]]<br />
<br />
Select Add Track and connect the pins connected by the yellow line. Try to follow a convention, like a grid, to make things easier on more complicated boards. The default track width should be fine for most purposes. If there will be a lot of current flowing on the track, you should make it wider by right-clicking and choosing Properties. Tracks can be as wide as you want, within the spacing constraints. Ground tracks are often chosen to be as wide as possible.<br />
<br />
[[Image:add_trace.jpg]]<br />
<br />
A good convention is to have traces running vertically be on one side of the board, and traces running horizontally be on the other. Select a portion of the trace, right click and select Properties. Here you can change the layer of the board and the width of the trace.<br />
<br />
[[Image:edit_trace.jpg]]<br />
<br />
Notice how the traces are automatically connected with a via to maintain connection.<br />
<br />
[[Image:automatic_via.jpg]]<br />
<br />
Traces do not have to be drawn orthogonally. While drawing a trace, right click and select segment mode. These different modes will draw traces in a different way. Miter is the most common.<br />
<br />
[[Image:miter_trace.jpg]]<br />
<br />
An orthogonally drawn trace can be turned into a miter by double clicking the corner and dragging the corner in.<br />
<br />
Use the Add Via button and Add track to complete the circuit. You need to add these vias to make the power and ground connections because they do not translate from the Schematic Schema to the PCB. If you were to make this board, you would solder wires to these vias to supply power.<br />
<br />
[[Image:add_via.jpg]]<br />
<br />
Add some text reminders to the silkscreen layer to remind yourself where to solder the power and ground, what direction the LED goes, and what resistor to use.<br />
<br />
[[Image:add_text.jpg]]<br />
<br />
When your board is complete, use the Design Rule Check to automatically look for errors in connecting pins and accidentally crossed traces. You can check all the boxes.<br />
<br />
[[Image:design_rule_check.jpg]]<br />
<br />
Go to Settings, PCB Configuration to make your board bigger or smaller or change other properties. You can also click on specific edges of your board to resize the board, or use the arrow to drag a box around all the components to select them and drag them around inside the board.<br />
<br />
[[Image:pcb_config.jpg]]<br />
<br />
Use this document, [[Media:Student Order Process updated 4-14.pdf|Student Order Process]], to purchase a board with the $33 Student Discount. (Maximum board size for this special is 60 square inches.)<br />
<br />
Go back to PCB Configuration and Enter Student in the Special Requirements section.<br />
<br />
[[Image:student_order.jpg]]<br />
<br />
Go to Output, Submit Order. PCB Artist will perform a Design Rule Check. After that, enter the Promotion Code 33each. If you do not have an Advanced Circuits account, go to [http://www.4pcb.com 4pcb.com] and make one. Then you may order your board.<br />
<br />
[[Image:submit_order.jpg]]<br />
<br />
==Adding a Component to the Library==<br />
<br />
A majority of the time the component you want to use is not in the existing libraries, therefore, you have to create a component. A component consists of a schematic symbol, a pcb symbol and a mapping between the pins of the schematic symbol and the pcb symbol. This section details how to create a component using the SN74HC04 Hex Inverter as an example. <br />
<br />
===Create a Schematic Symbol===<br />
<br />
With PCBArtist open (it doesn't matter if any schematics or pcb files are open), click on the blue book at the top of the screen. <br />
<br />
[[Image:PCBArtist_BlueBook.jpg]]<br />
<br clear=all> <br />
This opens the Library Manager of schematic symbols, pcb symbols and components. It will look something similar to the image below.<br />
<br />
[[Image:PCBArtist_LibraryManager.jpg]]<br />
<br clear=all><br />
<br />
To make a component, we first need to download the datasheet of the component. Download the datasheet for the Hex Inverter [[Media:SN74HC04Inverter.pdf|here]].<br />
<br clear=all><br />
<br />
On the first page of the datasheet, we can find a schematic of the component for SN74HC04. We will use this schematic to make our schematic in PCB Artist. <br />
*In PCBArtist, click on the 'Schematic Tab' of the Library Manager.<br />
*Click on the 'Wizard' Button in the middle of the window to begin creating a schematic symbol. <br />
<br />
This wizard will help you make your schematic symbols. <br />
*Click 'Next'<br />
[[Image:PCBArtist_Sch_Start.jpg]]<br />
<br clear=all><br />
<br />
*Choose English with a precision of 1 (these are the units for the symbol)<br />
*Click 'Next'<br />
[[Image:PCBArtist_Sch_Tech.jpg]]<br />
<br clear=all><br />
<br />
*Next we are going to choose the type of symbol. Our chip has pins on two sides so select the rectangle with pins on two sides (the first one).<br />
*Select the 'center' option for Origin. If you decide to rotate the symbol, it will rotate about this point.<br />
*Select the top left corner for Component Name. This indicates where the component name will be placed. <br />
*Click 'Next'<br />
[[Image:PCBArtist_Sch_SymbolType.jpg]]<br />
<br clear=all><br />
<br />
*In the next window, you can choose how you want the pins to be represented, the size of these and the length of the lines going to the pins. Leave these with the default values. <br />
*Click 'Next'<br />
[[Image:PCBArtist_Sch_Styles.jpg]]<br />
<br clear=all><br />
The next window determines the number of pins on each side of the component and some dimensions. <br />
*Enter 7 for both Pins on left and Pins on Right.<br />
*The other dimensions are not as important for the schematic, leave these as the default. (They can be editted later if too big or too small).<br />
*The last part on this page describes how the pins are numbered. Since the schematic on the datasheet spirals the numbers, select the 'Spiral Round' option.<br />
*Click 'Next'<br />
[[Image:PCBArtist_Sch_Pins.jpg]]<br />
<br clear=all><br />
<br />
The last window is where you name the symbol and save it to a specific library.<br />
*Name the component SN74HC04_Inverter and save it in the ME333_Lab2 Library that you downloaded above.<br />
*Click Finish<br />
You have now created a schematic symbol. If you checked the 'Edit the symbol now' box, you can see the schematic and edit if you want. We won't do any editting right now though.<br />
[[Image:PCBArtist_Sch_Finish.jpg]]<br />
<br clear=all><br />
<br />
===Create a PCB Symbol===<br />
The next step is to create a pcb symbol for the component. This will be the footprint on your PCB. Look at the datasheet again and find the Plastic Dual-In Line Package footprint for the SN74HC04 (page 10). This tells us the dimensions we need for our footprint.<br />
<br />
*Click on the Library Manager (blue book).<br />
*Go to the PCB Symbols tab.<br />
*Click on the Wizard again.<br />
*Click Next on the start up window.<br />
*On the Technology window, choose English with 'thou' as the unit and precision of 1. We choose this because it is the smallest unit on the datasheet for the English units.<br />
<br />
The next window is the footprint type.<br />
* Select DIP type (the top left).<br />
* For Origin - select center<br />
* For Name - select center - this is where the name will be displayed on the PCB<br />
*Click 'Next'<br />
[[Image:PCBArtist_PCB_FootPrint.jpg]]<br />
<br clear=all><br />
<br />
The next window describes the pads on the PCB. This window will be different for the different types of chips.<br />
*Choose 14 pads<br />
*For the Pad Style, choose 'Round' and leave Style blank.<br />
The units are in thousands of inches<br />
*e = 100 for 0.1 in pitch (as shown on the datasheet).<br />
*E = 400. We can put this value between 325 and 430 as shown on the datasheet, but we also know that the chip can plug into a breadboard (spacings of 0.1 in), so 400 thou is a good number. <br />
*PW = 60 and HD = 30. HD refers to the hole diameter for the pin. The pin diameter is around 20 thou as shown on the datasheet, so 30 thou is a good number with extra space. PW is the pad width (extra copper) surrounding the hole. Extra 30 thou is generally a good pad width.<br />
*Choose Pin 1 Position to be the lower left corner (same as in the footprint on the datasheet)<br />
*We don't want the pin numbers to be displayed on the PCB so select Hide for the Pin Numbers<br />
*Pin number is counter clockwise.<br />
*Click Next<br />
[[Image:PCBArtist_PCB_Pads.jpg]]<br />
<br clear=all><br />
<br />
The next window describes the silk screen (the writing on top of the PCB).<br />
*Click Yes<br />
*Layer - Top Silkscreen<br />
*Inside pads by 25 (sometimes you may want the silk screen to be outside the pads, but it is really your choice)<br />
*Include Marks - Notch at end (12.5) --> this helps you figure out the orientation of the component<br />
*Click Next<br />
[[Image:PCBArtist_PCB_SilkScreen.jpg]]<br />
<br />
<br clear=all><br />
The next window asks if you want a placement shape (rectangle) around the entire component. We are not going to include this.<br />
*Click No<br />
*Click Next<br />
<br />
*On the final window, name it SN74HC04_Inverter (same name as the schematic) and save it in the ME333 Library.<br />
*Click Finish<br />
<br />
You have now created a PCB footprint.<br />
<br />
<br clear=all><br />
<br />
===Create a Component===<br />
We created a schematic symbol and a pcb symbol above. Now we are going to create a component to link the two symbols. <br />
<br />
*Click on the Library Manager and go to the Component tab.<br />
*Click on the Wizard button<br />
*Click Next on the opening window<br />
<br />
The first window allows you to select what type of component you are making. You can make a Normal Component (both Schematic and PCB), only a Schematic component or only a PCB component.<br />
*Select Normal Component<br />
*Click Next<br />
<br />
This windows contains the details for the component.<br />
*Name = SN74HC04_Inverter (same as the symbols)<br />
*Package - type DIP (this really doesn't matter)<br />
*Default Reference - choose IC for integrated circuit<br />
*Component pins - 14<br />
*Number of Gates - 1 - this is the number of times your schematic symbol is needed for the component<br />
*Click Next<br />
[[Image:PCBArtist_Component_Details.jpg]]<br />
<br clear=all><br />
<br />
In the next window you select the schematic symbol that you want to use<br />
*Select SN74HC04_Inverter from the ME333 Library<br />
*Click Next<br />
In the PCB window, you select the pcb symbol that you want to use<br />
*Select SN74HC04_Inverter from the ME333 Library<br />
*Click Next<br />
<br />
The next window assigns the pins from the schematic to the pcb. <br />
Since we choose our pins in the schematic and pcb to have the same order:<br />
*Select Assign 1-to-1 button to assign Pin 1 Schematic to Pin 1 PCB ... etc<br />
We can also choose to add names to each pin.<br />
*Assign the pins names (1A,1Y,GND..etc) to each of the pins as shown on the datasheet.<br />
*Click Next<br />
[[Image:PCBArtist_Component_Pins.jpg]]<br />
<br clear=all><br />
<br />
On the final window:<br />
*Click Save the component to the library --> Put it in the ME333 library<br />
If you want to edit the component now (check the next box)<br />
*Click finish<br />
<br />
You have now created a component and can use it with schematics and pcbs.<br />
<br />
== Practice Making a PCB ==<br />
After reviewing this tutorial, you are ready to [[Practice Making a PCB]].</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_E-puck_Code_(outdated)&diff=14288RGB Swarm Robot Project E-puck Code (outdated)2009-09-04T18:12:18Z<p>Sam Bobb: /* To Do */</p>
<hr />
<div>This page documents the e-puck code for the RGB Sensing Swarm Robotics project. The code on the e-puck was written in C and compiled using Microchip's MPLAB C Compiler for dsPIC DSCs (student version). <br />
<br />
This code is a branch of the [[Swarm Project E-puck Code]].<br />
<br />
==Tasks==<br />
<br />
===Complete===<br />
* Got ADC working for color sensor<br />
** Set up filtering to address projector PWM<br />
* Restructured code to make more modular.<br />
** Split dsPIC_XBeePackets and wheel_speed_coordinator into h and c files<br />
** Pulled packet assembling code out of main and created send_packet() function in send_packet.h/c.<br />
** Pulled a bunch of variables and defines (''NUM_DATA_SETS, NUMBERS_PER_SET, DATATYPE_BYTELENGTH , DATA_ARRAY_LENGTH , ADDITIONAL_NUMS, notRTS, T1_INT_FLAG, x_i, u_i, w_i, x_sum, w_sum, MAX_WHEEL_V_TICKS, deadband, COMMR, SAFEDIST, MINDIST, u_x_ideal, u_y_ideal, x_motion_integral, y_motion_integral, SQUARE'') that were scattered across h files into global_vars.h/c. Makes it easy to include them in a particular file with the ''extern'' keyword.<br />
* Added color_cal() function in color_cal.h/c<br />
* Added wheelSpeedSingleBot to wheel_speed_coordinator<br />
<br />
===To Do===<br />
* Finish color_cal<br />
* Improve the vision system position information updater.<br />
* Replace wheelSpeedSingleBot with the three step move controller from NUtest.c<br />
* Implement new algorithm from paper<br />
<br />
==Project Package==<br />
The source code for the project is available here: [[Media:RGB_Swarm_Puck_Code_working_version.zip]]. Open swarm_epucks.mcw and you should be good to go.<br />
<br />
==Description of the files and functions==<br />
<br />
===global_vars(.c/.h)===<br />
* .c/.h: declare and define global variables and macros<br />
====Packet Length Constants====<br />
These variables determine the length of the XBee packets. See [[Swarm_Robot_Project_Documentation#Data_Frame|Data Frame]] and the section on XBee API packets in the XBee manual for further clarification.<br />
<br />
Much of this is still sending integral consensus estimator data. This can be removed or replaces with data needed for decentralized color sensing. <br />
<br />
=====#define NUM_DATA_SETS 5 ===== <br />
Number of statistics on which you are running the consensus estimator. This this particular case, 5. (Ix, Iy, Ixx, Ixy, Iyy)<br />
<br />
=====#define NUMBERS_PER_SET 2=====<br />
Number of variables in each data set (see above) that the consensus estimator needs to transmit to other agents. In this case, 2 because there is <tt>x_i</tt> and <tt>w_i</tt> for each statistic.<br />
<br />
=====#define DATATYPE_BYTELENGTH 4 =====<br />
Number of bytes in the data type (used in the consensus estimator (float = 4 bytes long). This is important because we need to split the numbers into individual bytes to be able to send them out the serial port.<br />
<br />
=====#define DATA_ARRAY_LENGTH (NUM_DATA_SETS*NUMBERS_PER_SET)=====<br />
Total number of data variables needed for the consensus estimator. In this case, it is 5*2=10.<br />
<br />
=====#define ADDITIONAL_NUMS 8=====<br />
Additional number of data to be appended to data array. It is 5 in this case, so that we can append <br />
# Robot X coordinate<br />
# Robot Y coordinate<br />
# Robot Theta orientation<br />
# Robot left wheel speed<br />
# Robot right wheel speed<br />
# Sensor Red Value<br />
# Sensor Green Value<br />
# Sensor Blue Value<br />
<br />
===main(.c/.h)===<br />
* .c: This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines. <br />
* .h: Contains variables, function prototypes, and delay function needed for main.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void)=====<br />
Timer1 ISR. Sets T1_INT_FLAG which provides timing for the main loop.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)=====<br />
UART2 receive ISR. Runs with the XBee receives packets. The switch/case structure handles the packets.<br />
<br />
The current handling of coordinate data from the vision system: <br />
<br />
<code><pre><br />
case 0: //coordinate data<br />
e_set_configuration(<br />
InPacket.data[1].dataFloat,<br />
InPacket.data[2].dataFloat, <br />
InPacket.data[3].dataFloat);<br />
break;<br />
</pre></code><br />
<br />
Needs to be improved. Suggested new function:<br />
<br />
* Get vision system data<br />
* Check if it's wildly off from current puck estimate<br />
** If not, update, clear log of rejected packets<br />
** If so, reject and log<br />
*** If we have rejected enough packets (some threshold) and they've all been in similar place (within tolerance), we can assume the puck is wrong and vision system is right. In this case, update with vision system data and clear rejected data log.<br />
<br />
=====int main(void)=====<br />
Setup functions and main loop.<br />
<br />
===color_cal(.c/.h)===<br />
* .c: Contains void calibrate_color(void) function to run the calibration routine.<br />
* .h: Contains function prototype and constant definitions for calibrate_color.<br />
<br />
=====void calibrate_color(void)=====<br />
This function runs the color calibration routine. Eventually this should be turned into a separate e-puck command from the vision system. Currently it just runs the puck through a zig zag pattern on the floor and sends packets. You can collect these with the data logger and process them in matlab. From this, you should be able to get a calibration function.<br />
<br />
'''To do:'''<br />
* Add to the pattern. Need to collect more data.<br />
* Possibly have the puck store data, find a best fit, and create the calibration function on board.<br />
* Store the calibration function in the EEPROM (flash memory) so it's non-volatile. The coefficients of function will be specific to each puck, so it would be nice to not have to program each puck, each time you change the battery.<br />
<br />
===dsPIC_XBeePackets(.c/.h)===<br />
* .c/.h: Contains functions and data structures for assembling and receiving XBee packets.<br />
<br />
=====int readPacket( void)=====<br />
Parses XBee data from UART2. Makes it accessable in the ''InPacket'' struct.<br />
<br />
The UART2 receive ISR takes error codes from this function and flashes the corresponding puck LED (absolute value of the error code).<br />
<br />
Error codes are:<br />
<br />
<code><pre><br />
//////////////ERROR CODES/////////////////<br />
// <br />
// -1: Timeout waiting for UART2<br />
// -2: Start delimiter wrong<br />
// -3: Checksum Error<br />
// -4: UART2 not ready at beginning<br />
// -5: API_ID unidentified<br />
// -6: Send Packet CCA failuue<br />
// -7: Modem Status packet failure<br />
//////////////////////////////////////////<br />
</pre></code><br />
<br />
===send_packet(.c/.h)===<br />
* .c: Contains the void send_packet(void) function which fills an array with data and calls the needed XBee functions to send a packet.<br />
* .h: Contains function prototype.<br />
<br />
=====void send_packet(void)=====<br />
* Creates ''packet'' array.<br />
* Adds consensus estimator data to the array.<br />
* Adds robot statistics to the array.<br />
* Adds color sensor values to the array.<br />
* Asserts flow control line to stop XBee from sending<br />
* Calls assemblePacket to send the packet.<br />
* Deasserts flow control; Xbee can send again.<br />
<br />
<br />
The function e_get_acc_filtered returns a running average of the acc specified. Syntax is:<br />
<br />
return value = e_get_acc_filtered(acc_channel, number of samples to average)<br />
<br />
The number of samples averaged must be less than ACC_SAMP_NB as defined in e_ad_conv.h.<br />
<br />
<code><pre><br />
packet[i]=e_get_acc_filtered(2, 136); // red<br />
i++;<br />
packet[i]=e_get_acc_filtered(1, 136); // green<br />
i++;<br />
packet[i]=e_get_acc_filtered(0, 136); // blue<br />
</pre></code><br />
<br />
===PI_consensus_estimator(.h)===<br />
* .h: Contains functions and data structures for the PI consensus estimator. <br />
* This will probably be replaced by the algorithm for sensor consensus. <br />
<br />
===wheel_speed_coordinator(.c/.h)===<br />
* .c: Contains functions for robot motion control<br />
* .h: Function prototypes and variabls.<br />
<br />
=====void wheelSpeed(int *vL, int *vR)=====<br />
Return needed wheel speeds for the inertial consensus estimator based on the group goal.<br />
<br />
=====void wheelSpeedSingleBot(float gotox, float gotoy, int *vL, int *vR)=====<br />
Return needed wheel speed to get this individual bot to (gotox, gotoy). It's a hacked fix. Should be replaced with the 3 step motion controller from NUtest.c.<br />
<br />
===e_acc(.c/.h)===<br />
* .c: Functions for reading the accelerometers (which is the color sensor).<br />
* .h: Function prototypes.<br />
<br />
This is original e-puck library code with the following modifications:<br />
<br />
<code><pre><br />
//changed by Sam, July 10, default offset is 2000. we want 0 for RGB sensor.<br />
static int centre_z = 0; //zero value for z axe<br />
</pre></code><br />
<br />
=====int e_get_acc_filtered(unsigned int captor, unsigned int filter_size)=====<br />
<br />
===e_ad_conv(.c/.h)===<br />
Set up the ADCs on the puck. Original e-puck library code, with the following modifications<br />
* .h: Define constants and functional prototypes<br />
<br />
MIC_SAMP_FREQ sets the baseline sampling frequency for the ADC, everything else must be a fraction of this. 16384 Hz is the highest possible.<br />
<code><pre><br />
#define MIC_SAMP_FREQ 16384.0 <br />
</pre></code><br />
<br />
ACC_PROX_SAMP_FREQ sets the sampling frequency for the accelerometers (color sensor). We found in testing that the puck become non-responsive with this set to 8192 Hz or 16384 Hz.<br />
<code><pre><br />
// sampling frequency for the accelerometres and proximetres<br />
//#define ACC_PROX_SAMP_FREQ 256.0 // WARNING: should be a fraction of MIC_SAMP_FREQ<br />
#define ACC_PROX_SAMP_FREQ 4096 // to ensure a good timing precision<br />
// So your options are: 1 2 4 8 16 32 64 128 <br />
// 256 512 1024 2048 4096 8192 16384<br />
</pre></code><br />
<br />
ACC_SAMP_NB is the number of samples to store. We can do an average of ''up to'' this many samples. This is set to 140 so we can average 136 samples, which is 4 projector periods.<br />
<code><pre><br />
#define ACC_SAMP_NB 140 // number of accelerometer samples to store<br />
</pre></code><br />
<br />
* .c: Functions and interrupt service routines for ADCs. Original e-puck library, no modifications.<br />
<br />
=====e_init_ad_scan(ALL_ADC)=====<br />
Call to setup ADC and have it work in the background. Use e_acc functions to access data.<br />
<br />
===e_init_port(.c/.h)===<br />
* .c/.h: Initializes the ports on the e-puck. File is from the standard e-puck library. <br />
<br />
=====e_init_ports(void)=====<br />
This function sets up ports on the e-puck. Call before using any ports.<br />
<br />
===e_led(.c/.h)===<br />
* .c/.h: This is a standard e-puck library file that contains functions for manipulating LEDs.<br />
<br />
=====void e_set_led(unsigned int led_number, unsigned int value)=====<br />
Set led_number (0-7) to value (0=off 1=on higher=inverse).<br />
<br />
[[Image:e-puck_LED_numbering.png|thumb|left]]<br />
<br />
<br clear='all'><br />
<br />
===e_motors_swarm(.c/.h)===<br />
* .c/.h: This file is a modified version of the e_motors.h e-puck library file. This version keeps track of the robot's position and orientation, and the motor stepping function contains code to update the robot's position when the wheels turn. <br />
* The functions like e_rotate and e_translate have been removed and this version is not dependent on e_agenda. <br />
<br />
=====#define POINT_OFFSET -31.75 =====<br />
In addition to constants like wheel radius and wheel base, the offset between the center point of the bot and the point we're driving is set in e_motors_swarm.h. -31.75 mm is the distance betwene the center and the color sensor. So technically, we're driving backwards.<br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T5Interrupt(void)=====<br />
Timer5 ISR, interrupt for left motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos + cval*LINSTEP;<br />
ypos = ypos + sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T4Interrupt(void)=====<br />
Timer4 ISR, interrupt for right motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos - cval*LINSTEP;<br />
ypos = ypos - sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void e_init_motors(void)=====<br />
Call this function before other motor functions to initialize the motors.<br />
<br />
=====void e_set_speed_left(int motor_speed)/void e_set_speed_right(int motor_speed)=====<br />
Set the motor speed in steps/second.<br />
<br />
=====void e_get_configuration(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the center reference point.<br />
<br />
=====void e_set_configuration(float x, float y, float theta)=====<br />
Sets x, y, theta to values. This is used to overwrite the puck position/orientation estimates with data from the vision system.<br />
<br />
=====void e_get_configuration_front(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the front reference point (used for motor control).<br />
<br />
<br />
[[Category:SwarmRobotProject]]<br />
[[Category:e-puck]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_E-puck_Code_(outdated)&diff=14287RGB Swarm Robot Project E-puck Code (outdated)2009-09-04T18:11:59Z<p>Sam Bobb: /* To Do */</p>
<hr />
<div>This page documents the e-puck code for the RGB Sensing Swarm Robotics project. The code on the e-puck was written in C and compiled using Microchip's MPLAB C Compiler for dsPIC DSCs (student version). <br />
<br />
This code is a branch of the [[Swarm Project E-puck Code]].<br />
<br />
==Tasks==<br />
<br />
===Complete===<br />
* Got ADC working for color sensor<br />
** Set up filtering to address projector PWM<br />
* Restructured code to make more modular.<br />
** Split dsPIC_XBeePackets and wheel_speed_coordinator into h and c files<br />
** Pulled packet assembling code out of main and created send_packet() function in send_packet.h/c.<br />
** Pulled a bunch of variables and defines (''NUM_DATA_SETS, NUMBERS_PER_SET, DATATYPE_BYTELENGTH , DATA_ARRAY_LENGTH , ADDITIONAL_NUMS, notRTS, T1_INT_FLAG, x_i, u_i, w_i, x_sum, w_sum, MAX_WHEEL_V_TICKS, deadband, COMMR, SAFEDIST, MINDIST, u_x_ideal, u_y_ideal, x_motion_integral, y_motion_integral, SQUARE'') that were scattered across h files into global_vars.h/c. Makes it easy to include them in a particular file with the ''extern'' keyword.<br />
* Added color_cal() function in color_cal.h/c<br />
* Added wheelSpeedSingleBot to wheel_speed_coordinator<br />
<br />
===To Do===<br />
* Finish color_cal<br />
* Improve the vision system position information updater.<br />
* Replace wheelSpeedSingleBot with the three step move controller from NUtest.c<br />
* Implement new algorithm<br />
<br />
==Project Package==<br />
The source code for the project is available here: [[Media:RGB_Swarm_Puck_Code_working_version.zip]]. Open swarm_epucks.mcw and you should be good to go.<br />
<br />
==Description of the files and functions==<br />
<br />
===global_vars(.c/.h)===<br />
* .c/.h: declare and define global variables and macros<br />
====Packet Length Constants====<br />
These variables determine the length of the XBee packets. See [[Swarm_Robot_Project_Documentation#Data_Frame|Data Frame]] and the section on XBee API packets in the XBee manual for further clarification.<br />
<br />
Much of this is still sending integral consensus estimator data. This can be removed or replaces with data needed for decentralized color sensing. <br />
<br />
=====#define NUM_DATA_SETS 5 ===== <br />
Number of statistics on which you are running the consensus estimator. This this particular case, 5. (Ix, Iy, Ixx, Ixy, Iyy)<br />
<br />
=====#define NUMBERS_PER_SET 2=====<br />
Number of variables in each data set (see above) that the consensus estimator needs to transmit to other agents. In this case, 2 because there is <tt>x_i</tt> and <tt>w_i</tt> for each statistic.<br />
<br />
=====#define DATATYPE_BYTELENGTH 4 =====<br />
Number of bytes in the data type (used in the consensus estimator (float = 4 bytes long). This is important because we need to split the numbers into individual bytes to be able to send them out the serial port.<br />
<br />
=====#define DATA_ARRAY_LENGTH (NUM_DATA_SETS*NUMBERS_PER_SET)=====<br />
Total number of data variables needed for the consensus estimator. In this case, it is 5*2=10.<br />
<br />
=====#define ADDITIONAL_NUMS 8=====<br />
Additional number of data to be appended to data array. It is 5 in this case, so that we can append <br />
# Robot X coordinate<br />
# Robot Y coordinate<br />
# Robot Theta orientation<br />
# Robot left wheel speed<br />
# Robot right wheel speed<br />
# Sensor Red Value<br />
# Sensor Green Value<br />
# Sensor Blue Value<br />
<br />
===main(.c/.h)===<br />
* .c: This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines. <br />
* .h: Contains variables, function prototypes, and delay function needed for main.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void)=====<br />
Timer1 ISR. Sets T1_INT_FLAG which provides timing for the main loop.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)=====<br />
UART2 receive ISR. Runs with the XBee receives packets. The switch/case structure handles the packets.<br />
<br />
The current handling of coordinate data from the vision system: <br />
<br />
<code><pre><br />
case 0: //coordinate data<br />
e_set_configuration(<br />
InPacket.data[1].dataFloat,<br />
InPacket.data[2].dataFloat, <br />
InPacket.data[3].dataFloat);<br />
break;<br />
</pre></code><br />
<br />
Needs to be improved. Suggested new function:<br />
<br />
* Get vision system data<br />
* Check if it's wildly off from current puck estimate<br />
** If not, update, clear log of rejected packets<br />
** If so, reject and log<br />
*** If we have rejected enough packets (some threshold) and they've all been in similar place (within tolerance), we can assume the puck is wrong and vision system is right. In this case, update with vision system data and clear rejected data log.<br />
<br />
=====int main(void)=====<br />
Setup functions and main loop.<br />
<br />
===color_cal(.c/.h)===<br />
* .c: Contains void calibrate_color(void) function to run the calibration routine.<br />
* .h: Contains function prototype and constant definitions for calibrate_color.<br />
<br />
=====void calibrate_color(void)=====<br />
This function runs the color calibration routine. Eventually this should be turned into a separate e-puck command from the vision system. Currently it just runs the puck through a zig zag pattern on the floor and sends packets. You can collect these with the data logger and process them in matlab. From this, you should be able to get a calibration function.<br />
<br />
'''To do:'''<br />
* Add to the pattern. Need to collect more data.<br />
* Possibly have the puck store data, find a best fit, and create the calibration function on board.<br />
* Store the calibration function in the EEPROM (flash memory) so it's non-volatile. The coefficients of function will be specific to each puck, so it would be nice to not have to program each puck, each time you change the battery.<br />
<br />
===dsPIC_XBeePackets(.c/.h)===<br />
* .c/.h: Contains functions and data structures for assembling and receiving XBee packets.<br />
<br />
=====int readPacket( void)=====<br />
Parses XBee data from UART2. Makes it accessable in the ''InPacket'' struct.<br />
<br />
The UART2 receive ISR takes error codes from this function and flashes the corresponding puck LED (absolute value of the error code).<br />
<br />
Error codes are:<br />
<br />
<code><pre><br />
//////////////ERROR CODES/////////////////<br />
// <br />
// -1: Timeout waiting for UART2<br />
// -2: Start delimiter wrong<br />
// -3: Checksum Error<br />
// -4: UART2 not ready at beginning<br />
// -5: API_ID unidentified<br />
// -6: Send Packet CCA failuue<br />
// -7: Modem Status packet failure<br />
//////////////////////////////////////////<br />
</pre></code><br />
<br />
===send_packet(.c/.h)===<br />
* .c: Contains the void send_packet(void) function which fills an array with data and calls the needed XBee functions to send a packet.<br />
* .h: Contains function prototype.<br />
<br />
=====void send_packet(void)=====<br />
* Creates ''packet'' array.<br />
* Adds consensus estimator data to the array.<br />
* Adds robot statistics to the array.<br />
* Adds color sensor values to the array.<br />
* Asserts flow control line to stop XBee from sending<br />
* Calls assemblePacket to send the packet.<br />
* Deasserts flow control; Xbee can send again.<br />
<br />
<br />
The function e_get_acc_filtered returns a running average of the acc specified. Syntax is:<br />
<br />
return value = e_get_acc_filtered(acc_channel, number of samples to average)<br />
<br />
The number of samples averaged must be less than ACC_SAMP_NB as defined in e_ad_conv.h.<br />
<br />
<code><pre><br />
packet[i]=e_get_acc_filtered(2, 136); // red<br />
i++;<br />
packet[i]=e_get_acc_filtered(1, 136); // green<br />
i++;<br />
packet[i]=e_get_acc_filtered(0, 136); // blue<br />
</pre></code><br />
<br />
===PI_consensus_estimator(.h)===<br />
* .h: Contains functions and data structures for the PI consensus estimator. <br />
* This will probably be replaced by the algorithm for sensor consensus. <br />
<br />
===wheel_speed_coordinator(.c/.h)===<br />
* .c: Contains functions for robot motion control<br />
* .h: Function prototypes and variabls.<br />
<br />
=====void wheelSpeed(int *vL, int *vR)=====<br />
Return needed wheel speeds for the inertial consensus estimator based on the group goal.<br />
<br />
=====void wheelSpeedSingleBot(float gotox, float gotoy, int *vL, int *vR)=====<br />
Return needed wheel speed to get this individual bot to (gotox, gotoy). It's a hacked fix. Should be replaced with the 3 step motion controller from NUtest.c.<br />
<br />
===e_acc(.c/.h)===<br />
* .c: Functions for reading the accelerometers (which is the color sensor).<br />
* .h: Function prototypes.<br />
<br />
This is original e-puck library code with the following modifications:<br />
<br />
<code><pre><br />
//changed by Sam, July 10, default offset is 2000. we want 0 for RGB sensor.<br />
static int centre_z = 0; //zero value for z axe<br />
</pre></code><br />
<br />
=====int e_get_acc_filtered(unsigned int captor, unsigned int filter_size)=====<br />
<br />
===e_ad_conv(.c/.h)===<br />
Set up the ADCs on the puck. Original e-puck library code, with the following modifications<br />
* .h: Define constants and functional prototypes<br />
<br />
MIC_SAMP_FREQ sets the baseline sampling frequency for the ADC, everything else must be a fraction of this. 16384 Hz is the highest possible.<br />
<code><pre><br />
#define MIC_SAMP_FREQ 16384.0 <br />
</pre></code><br />
<br />
ACC_PROX_SAMP_FREQ sets the sampling frequency for the accelerometers (color sensor). We found in testing that the puck become non-responsive with this set to 8192 Hz or 16384 Hz.<br />
<code><pre><br />
// sampling frequency for the accelerometres and proximetres<br />
//#define ACC_PROX_SAMP_FREQ 256.0 // WARNING: should be a fraction of MIC_SAMP_FREQ<br />
#define ACC_PROX_SAMP_FREQ 4096 // to ensure a good timing precision<br />
// So your options are: 1 2 4 8 16 32 64 128 <br />
// 256 512 1024 2048 4096 8192 16384<br />
</pre></code><br />
<br />
ACC_SAMP_NB is the number of samples to store. We can do an average of ''up to'' this many samples. This is set to 140 so we can average 136 samples, which is 4 projector periods.<br />
<code><pre><br />
#define ACC_SAMP_NB 140 // number of accelerometer samples to store<br />
</pre></code><br />
<br />
* .c: Functions and interrupt service routines for ADCs. Original e-puck library, no modifications.<br />
<br />
=====e_init_ad_scan(ALL_ADC)=====<br />
Call to setup ADC and have it work in the background. Use e_acc functions to access data.<br />
<br />
===e_init_port(.c/.h)===<br />
* .c/.h: Initializes the ports on the e-puck. File is from the standard e-puck library. <br />
<br />
=====e_init_ports(void)=====<br />
This function sets up ports on the e-puck. Call before using any ports.<br />
<br />
===e_led(.c/.h)===<br />
* .c/.h: This is a standard e-puck library file that contains functions for manipulating LEDs.<br />
<br />
=====void e_set_led(unsigned int led_number, unsigned int value)=====<br />
Set led_number (0-7) to value (0=off 1=on higher=inverse).<br />
<br />
[[Image:e-puck_LED_numbering.png|thumb|left]]<br />
<br />
<br clear='all'><br />
<br />
===e_motors_swarm(.c/.h)===<br />
* .c/.h: This file is a modified version of the e_motors.h e-puck library file. This version keeps track of the robot's position and orientation, and the motor stepping function contains code to update the robot's position when the wheels turn. <br />
* The functions like e_rotate and e_translate have been removed and this version is not dependent on e_agenda. <br />
<br />
=====#define POINT_OFFSET -31.75 =====<br />
In addition to constants like wheel radius and wheel base, the offset between the center point of the bot and the point we're driving is set in e_motors_swarm.h. -31.75 mm is the distance betwene the center and the color sensor. So technically, we're driving backwards.<br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T5Interrupt(void)=====<br />
Timer5 ISR, interrupt for left motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos + cval*LINSTEP;<br />
ypos = ypos + sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T4Interrupt(void)=====<br />
Timer4 ISR, interrupt for right motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos - cval*LINSTEP;<br />
ypos = ypos - sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void e_init_motors(void)=====<br />
Call this function before other motor functions to initialize the motors.<br />
<br />
=====void e_set_speed_left(int motor_speed)/void e_set_speed_right(int motor_speed)=====<br />
Set the motor speed in steps/second.<br />
<br />
=====void e_get_configuration(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the center reference point.<br />
<br />
=====void e_set_configuration(float x, float y, float theta)=====<br />
Sets x, y, theta to values. This is used to overwrite the puck position/orientation estimates with data from the vision system.<br />
<br />
=====void e_get_configuration_front(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the front reference point (used for motor control).<br />
<br />
<br />
[[Category:SwarmRobotProject]]<br />
[[Category:e-puck]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_Documentation&diff=14286RGB Swarm Robot Project Documentation2009-09-04T18:09:27Z<p>Sam Bobb: /* Projector */</p>
<hr />
<div>== Overview ==<br />
The swarm robot project has gone through several phases, with each phase focusing on different aspects of swarm robotics and the implementation of the project. This entry focuses on the most recent phase of the project, covering topics such as, but not limited to, '''Xbee Interface Extension Boards''', '''LED light boards''', and '''changes made to the Machine Vision Localization System''', and the overall conversion to LED boards and a controlled light environment. These entries help provide insight into setup and specific details to allow others to replicate or reproduce our results, and to provide additional information for those working on similar projects or this project at a later time. Other articles in the '''Swarm Robot Project''' category focus on topics such as the swarm theory and algorithms implemented, as well as previous phases of the project, such as motion control and consensus estimation. You may reach these articles and others by following the category link at the bottom of every page, or through this link - [[:Category:SwarmRobotProject|'''Swarm Robot Project''']].<br />
<br />
== Hardware ==<br />
<br />
<br />
<br />
===XBee Interface Extension Board Version 2===<br />
<br />
{|<br />
| [[Image:XBee_interface_extenstion_board_v1.gif|250px|thumb|alt=Traxmaker Image of the Previous Xbee Extension Board|Xbee Interface Extension Board Version]]<br />
| [[Image:IMG 1390-1-.jpg|300px|thumb|alt=Image of an e-Puck with the RGB Xbee Extension Board|e-Puck with previous board ]]<br />
| [[Image:XBee_interface_extenstion_board_v2.gif|vertical|250px|thumb|alt=Traxmaker Image of the Xbee Interface Exension Board Version 2|Xbee Interface Extension Board Version 2]]<br />
| <br />
|}<br />
<br />
====Previous Version====<br />
<br />
The previous version of XBee Interface Extension Board, designed by Michael Hwang.<br />
Its configuration is shown in the figure on the left, with an actual image of the board mounted on an e-Puck seen in the figure in the center. This version of the XBee Interface Board does not contain a color sensor in it. Details about this version of XBee Interface Extension Board, such as parts used and Traxmaker files can be found on the [[Swarm_Robot_Project_Documentation#Current_Version|Swarm Robot Project Documentation page]].<br />
<br clear="all"><br />
<br />
====Version 2====<br />
Th is the updated version of the Xbee board, or XBee Interface Extension Board Version 2. It is designed by Michael Hwang to accommodate further projects in the Swarm Robot Project. For this reason, the Xbee Interface Extension Board Version 2 has a color sensor circuit built in. The details of the color sensor circuit can be found in the color sensor section below. A copy of the Traxmaker PCB file for the Xbee Board Version 2 can be found at [[Media:epuck_xbee_board_v2.zip|'''Xbee Interface Extension Board Version 2''']].<br />
<br />
The RTS flow control line on the XBee is connected to the sel3 line of the e-puck. Although the CTS line is not connected to the sel2 pin in this board design, it can be easily connected with a jumper. <br />
<br />
The XBee Interface Extension Board Version 2 design was actually built and implemented on the e-puck #3. In order to see if there is any working problem in this board design, it is first tested with the other e-puck which uses the previous XBee Boards. <br />
<br />
The e-puck #3 upgraded with the new XBee board did not show any problem in communicating with other e-pucks. According to the goal defined, all e-pucks, including e-puck #3, locate themselves to the desired location.<br />
<br clear="all"><br />
=====Color Sensor Circuit=====<br />
{|<br />
| [[Image:color_sensor_circuit_diagram_v1_R.gif|300px|thumb|Red Color Sensor Circuit]]<br />
| [[Image:color_sensor_circuit_diagram_v1_G.gif|315px|thumb|Green Color Sensor Circuit]]<br />
| [[Image:color_sensor_circuit_diagram_v1_B.gif|300px|thumb|Blue Color Sensor Circuit]]<br />
|}<br />
<br />
As you may draw from the circuit diagrams above, as each photodiode receives light, a certain amount of current start to flow through the photodiodes and generates a voltage across R<sub>1</sub> = 680K. Each photodiode is designed to detect the certain range of wavelength of the light, and the amount of current flowing through the photodiodes is determined according to the amount of the corresponding light to each photodiode. The op-amp (LMC6484) takes the voltage generated across R<sub>1</sub> as the input signal, amplifying it by a ratio particular to the circuit. This ratio is also known as gain, and is defined by resistance of the potentiometer. The now amplified output is then sent to the analog digital converter, which on the e-Puck had been used as the X,Y, and Z axis accelerometers. This convenient, as each accelerometer axis can be used as a channel for the color sensors three colors. The converted signal can then be used to measure the response of the color sensor to light. The corresponding equation for the circuits illustrated above are as follows:<br />
<br />
<math>|V_o| = |V_i * \frac{R_2}{R_{pot}}|</math><br />
<br />
*R<sub>pot</sub> = resistance of the potentiometer (shown in the diagram)<br />
*R<sub>2</sub> = 100K (shown in the diagram)<br />
*V<sub>i</sub> = voltage across R<sub>1</sub> = 680K, which the op-amp takes as an input<br />
*V<sub>o</sub> = output signal amplified from the op-amp<br />
<br />
The gain of the color sensor circuits is approximately 20. Thus, the input voltage, V<sub>i</sub>, is amplified to be 20V<sub>i</sub>, which is V<sub>o</sub>. As mentioned above, the gain can be adjusted properly by controlling the resistance of the potentiometer.<br />
<br />
As shown in the circuit diagram on the left, the siganl from the red photodiode goes into the pin #5, and the amplified signal is sent out through the pin # 7. Similarly, the signal from the green photodiode goes into the pin #3 and it is sent out from pin #1 while the signal from the blue photodiode goes into the pin #12, and it is sent out from pin #14. <br />
<br />
Output Pins<br />
*Pin #7 - Amplified Red photodiode signal<br />
*Pin #1 - Amplified Green photodiode signal<br />
*Pin #14 - Amplified Blue photodiode signal<br />
<br />
=====Parts used=====<br />
Parts used in both the previous version and the new version of XBee Interface Extension Board<br />
*2x 10 pos. 2 mm pitch socket (Digikey S5751-10-ND) <br />
*LE-33 low dropout voltage regulator (Digikey 497-4258-1-ND) <br />
*2.2uF tantalum capacitor (Digikey 399-3536-ND) <br />
*2x Samtec BTE-020-02-L-D-A (Order directly from Samtec) <br />
*0.1"header pins for RTS and CTS pins (you can also use wire for a permanent connection) <br />
*2x 0.1" jumpers for connecting RTS and CTS pins if you used header pins(Digikey S9000-ND) <br />
<br />
Additional parts for new version of XBee Interface Extension Board<br />
*3x 100K resistors<br />
*3x 680K resistors<br />
*3x 10K potentiometer<br />
*3x 5pF capacitor<br />
*1x RGB color sensor (Order directly from HAMAMATSU, part#:s9032-02, [http://jp.hamamatsu.com/resources/products/ssd/pdf/s9032-02_kspd1067e03.pdf Datasheet])<br />
*1x High impedence op-amp LMC6484<br />
<br />
=====Future modifications=====<br />
As mentioned in the overview, the black dot patterns of e-pucks are replaced with new LED patterns by implementing LED pattern board at the top of each e-puck. Thus, in order for the color sensor to collect data properly, it is necessary to move the color sensor from the XBee Interface Extension Board to the LED pattern board so that nothing will block the color sensor. All other components for the color sensor circuit remains in the XBee Interface Extension Board and only the color sensor will be place in the LED pattern board. We can use a jumper to connect the color sensor placed at the LED pattern board to the color sensor circuit place in the XBee Interface Extension Board. The datails of this LED pattern Board will be presented at the section below.<br />
----<br />
<br />
===LED Pattern Board===<br />
[[Image:LED_pattern_board.gif|280px|right|thumb]]<br />
This is the LED pattern board, which was introduced for the RGB Swarm Robot Project. Currently, the unique black dot pattern of each e-puck was used for the machine vision system to recognize each e-puck. However, this black dot pattern requires a white background in order for the machine vision system to recognize e-pucks. The new LED pattern board uses LEDs with the proper brightness, instead of the black dot pattern. By doing so, the machine vision system can now recognize e-pucks on any background. The reason why this LED pattern is recognized on any background will be presented briefly in the Code section below. In addition, in order to apply this LED pattern to the machine vision system, we made a modification in code. This modification will also be presented in the Code Section below. The PCB file can be downloaded here: [[Media:LED_Pattern_Board.zip|'''LED Pattern Board''']]. This file contains the Traxmaker PCB files for an individual LED Pattern Board, as well as a 2x2 array, along with the necessary Gerber and drill files necessary for ordering PCBs.<br />
<br />
====LED Pattern Board Design====<br />
This LED Pattern Board is created using Traxmaker. This LED Board design can be downloaded here:<br />
Although we replaced the black dots with LEDs, we maintain each pattern of dots. The horizontal distance and the vertical distance between the two adjacent LEDs are both 0.8 inch. In order to reduce power consumption of the e-puck battery, we implement a seperate pair of AAA batteries to supply power to the LEDs. This LED board can be turned on and off by the switch.<br />
The millicandela rating of the LEDs used is 4850 mcd. In addition, this LED has diffused lens style. The reason to choose this LED is that it has a proper brightness and power consumption, and it is diffused so that the machine vision system can capture this LED in any places.The resistor used are 68.7 ohm. <br />
<br />
As mentioned in the XBee Interface Extension Board section, the color sensor has to be moved to this LED pattern board from the XBee Interface Extension Board so that nothing blocks the sensor. Thus, as you can see in the Figure on the left, the color sensor is place at the front, and each photodiode is connected to the 10 pin header. This header connects the color sensor on the LED pattern board to the remaining part of color sensor circuit on the XBee Interface Extension Board v2. <br />
<br />
====Parts used====<br />
*3x LED (Digikey 516-1697-ND): Some e-pucks require 4 LEDs since they have a pattern composed of 4 dots<br />
*3x 68.7 ohm resistors : Some e-pucks require 4 resistors since they have 4 LEDs<br />
*2x AAA Battery Holder (Digikey 2466K-ND)<br />
*1x Switch (Digikey CKN1068-ND)<br />
*1x RGB color sensor (Order directly from HAMAMATSU, part#:s9032-02)<br />
*1x 10 pos. 2 mm pitch socket (Digikey S5751-10-ND) <br />
<br />
====Tests====<br />
<br />
=====LED Distance vs Color Sensor Signal=====<br />
Tests need be done in order to note the affect of the LED light on the color sensor due to potential interference. The first experiment performed is designed to see how much interference will be caused as the distance between the LED and the color sensor changes.<br />
<br />
'''Setup and Results'''<br />
<br />
1. A white LED is used in this experiment because the white LED will cover the entire wavelengh ranges of the visible light. The experiment with the white LED can yield a general result, while the experiment with the colored LEDs will yield more specific result focused on the interference between the certain photodiode and the certain color.<br />
*LED: 18950 mcd (millicandela), digikey part number: C503B-WAN-CABBB151-ND<br />
<br />
2. The experiment was performed under the two conditions; with the ambient light and without the ambient light. <br />
<br />
3. The LED and the color sensor were placed at the same plane, and both are facing upward. <br />
<br />
4. Distance between the color sensor and the LED is increased by 0.25 inch each time from 1 inch to 2.5 inch.<br />
<br />
5. The amplified output, V<sub>o</sub> as shown in the circuit diagram above, of each photodiode is measured.<br />
<br />
With Ambient light<br />
[[Image:Distance_vs_output_with_room_light.gif|450px|left|thumb]]<br />
*Unit: Volt, V<br />
<br />
{| class="wikitable" border="3"<br />
|+'''Distance vs Amplified Output'''<br />
|-<br />
! Distance !! R !! G !! B <br />
|-<br />
| No LED|| 1 || 1.4 || 0.469<br />
|-<br />
| 1 inch || 1.259 || 1.716 || 0.832<br />
|-<br />
| 1.25 inch || 1.185 || 1.619 || 0.757<br />
|-<br />
| 1.5 inch || 1.135 || 1.529 || 0.669<br />
|-<br />
| 1.75 inch || 1.097 || 1.503 || 0.613<br />
|-<br />
| 2 inch || 1.086 || 1.481 || 0.589<br />
|-<br />
| 2.25 inch || 1.071 || 1.47 || 0.563<br />
|-<br />
| 2.5 inch || 1.06 || 1.453 || 0.546<br />
|}<br />
<br clear="all"><br />
Without the Ambient Light<br />
[[Image:Distance_vs_output_without_room_light.jpg|450px|left|thumb]]<br />
*Unit: Volt, V<br />
<br />
{| class="wikitable" border="3"<br />
|+'''Distance vs Amplified Output'''<br />
|-<br />
! Distance !! R !! G !! B <br />
|-<br />
| No LED|| 0.028 || 0.025 || 0.019<br />
|-<br />
| 1 inch || 0.244 || 0.221 || 0.223<br />
|-<br />
| 1.25 inch || 0.195 || 0.166 || 0.143<br />
|-<br />
| 1.5 inch || 0.162 || 0.123 || 0.097<br />
|-<br />
| 1.75 inch || 0.130 || 0.097 || 0.069<br />
|-<br />
| 2 inch || 0.102 || 0.077 || 0.054<br />
|-<br />
| 2.25 inch || 0.087 || 0.064 || 0.045<br />
|-<br />
| 2.5 inch || 0.073 || 0.056 || 0.039<br />
|}<br />
<br clear="all"><br />
As you can see in the two graphs above, the color sensor is affected by the light from the LED. The color sensor is most affectd by the LED when the LED is closest to it. As the distance between the LED and the color sensor increases, the interference decreases. When the color sensor is most affected by the LED under the presence of the room light, the output increases up to 25.9%, 22.6%, and 43.6 % of the original output. As the LED is 2.5 inch away from the color sensor, the output becomes very close to the original value. <br />
In this experiment, we see that the lights from LEDs can affect the color sensor. However, we used much brighter LED in this experiment than the ones we use in the LED pattern board. The brightness of the LED used in the experiment is 4 times larger than the ones in the LED pattern board. Thus, more experiment with the LEDs used in the LED pattern board is required.<br />
<br />
=====LED Angle vs Color Sensor Signal=====<br />
<br />
The second experiment is designed to see how much interference will be caused as the angle between LED and color sensor changes. Different from the first experiment, V<sub>i</sub>, the voltage before amplified, is mesured since amplified output, V<sub>o</sub>, easily reaches to the maximum.<br />
<br />
'''Setup and Results'''<br />
<br />
1. A white LED is used again in this experiment with the same reason above for the first experiment.<br />
*LED: 18950 mcd, Digikey part number: C503B-WAN-CABBB151-ND<br />
<br />
2. The experiment was performed under the two conditions; with the ambient light and without the ambient light. <br />
<br />
3. In this experiment, the distance between LED and color sensor is kept constant, 1 inch. <br />
<br />
4. Angle between LED and color sensor is increased by 15º each time from 0º to 90º.<br />
<br />
When the angle is 0º, the LED and the color sensor is placed at the same horizontal plane. The LED is facing toward the color sensor(this means that the LED is parallel to the horizontal plane with its head facing the color sensor, which is placed on the same horizontal plane), and the color sensor is facing upward. We increased the angle by 15º each time, and increasing amounts of light from the LED shines onto the color sensor. When the angle is 90º, the LED is right above the color sensor, facing the color sensor directly. This means that the LED and the color sensor are now on the same vertical line, and the LED is facing downward.<br />
<br />
5. The voltage before amplified, V<sub>i</sub> as shown in the circuit diagram above, of each photodiode is measured.<br />
* The reason to measure the volatage before amplified is that the output becomes too large after amplified.<br />
<br />
With the Ambient Light<br />
[[Image:Angle_vs_output_with_room_light.gif|450px|left|thumb]]<br />
*Unit: Volt, V<br />
<br />
{| class="wikitable" border="3"<br />
|+'''Angle vs Voltage Before Amplified'''<br />
|-<br />
! Angle !! R !! G !! B <br />
|-<br />
| 0º || 0.437 || 0.425 || 0.404<br />
|-<br />
| 15º || 0.475 || 0.470 || 0.451<br />
|-<br />
| 30º || 0.490 || 0.491 || 0.501<br />
|-<br />
| 45º || 0.505 || 0.506 || 0.520<br />
|-<br />
| 60º || 0.484 || 0.468 || 0.484<br />
|-<br />
| 75º || 0.457 || 0.453 || 0.440<br />
|-<br />
| 90º || 0.439 || 0.430 || 0.408<br />
|}<br />
<br clear="all"><br />
<br />
Without the Ambient Light<br />
[[Image:Angle_vs_output_without_room_light.jpg|450px|left|thumb]]<br />
*Unit: Volt, V<br />
<br />
{| class="wikitable" border="3"<br />
|+'''Angle vs Voltage Before Amplified'''<br />
|-<br />
! Angle !! R !! G !! B <br />
|-<br />
| 0º || 0.446 || 0.436 || 0.416<br />
|-<br />
| 15º || 0.454 || 0.491 || 0.461<br />
|-<br />
| 30º || 0.493 || 0.505 || 0.480<br />
|-<br />
| 45º || 0.512 || 0.521 || 0.520<br />
|-<br />
| 60º || 0.498 || 0.486 || 0.491<br />
|-<br />
| 75º || 0.498 || 0.492 || 0.487<br />
|-<br />
| 90º || 0.485 || 0.479 || 0.515<br />
|}<br />
<br clear="all"><br />
<br />
As the first experiment, two graph above shows that the color sensor is affected by the light from the LED. The color sensor is most affectd by the LED when the angle between two is 45º. The inteference increases as the angle goes to 45º, and reaches to the peak at 45º. Then it decreases as the angle goes to 90º. When the color sensor is most affected by the LED under the presence of the room light, the output increases upto 15.6%, 19.1%, and 28.7% of Vi. As angle becomes 90º, the output becomes very close to the value at the angle of 0º. The reason why the interference is reduced as the angle reaches 90º is that the ambient light presented are blocked by the LED board. When we perform this experiment, the LEDs are implemented on the LED plane. This LED plane blocks the light and make a shadow on the color sensor. Thus, the amount of light that the color sensor receives decreases. That is why the output becomes close to its original value while the angle increases.<br />
<br />
====Next Steps====<br />
The LED Pattern Board design above needs to be modified in the following parts.<br />
*The hole size for the LEDs has to increase so that it can accomodate the standoff of the LED chosen.<br />
*The hole size for the switch has to increase so that the switch can be completely inserted through the hole.<br />
*Currently, 10 pos 2mm pitch socket is used to connect the color sensor to the circuit using wires. Instead, the proper header for the color sensor has to be found to connect the color sensor and the circuit more conveniently.<br />
<br />
==Machine Vision Localization System Modification==<br />
Below is the documentation of changes made to the original machine vision localization system code to accommodate changed setup of the RGB Swarm Robot Project. This version of the code can be downloaded from the following link [[media:RGB_Machine_Vision_Localization_System.zip|'''RGB Machine Vision Localization System''']].<br />
<br />
===Compatibility Problem of Original Code with LEDs===<br />
The Machine Vision Localization System takes the real (color) image from the four cameras, and converts it into a grey-scale image. Then, using a threshold set in the machine vision code, the grey-scaled image is divided into black and white, and this black and white image is presented on the machine vision system computer screen. With the current set-up, the white background on the floor is presented as black, and black dot patterns on e-pucks are presented as white patterns. The system recognizes theses white dot patterns and identify e-pucks, and broadcasts the position coordinates to each e-puck via the Xbee Radio. For more information about the theory and operation of the system, look through the [[Machine Vision Localization System]] article.<br />
<br />
However, there is a problem with using black dot patterns to identify e-pucks. Since the machine vision system and code use a preset threshold to divide the grey image into black and white, black dot patterns are affected by the background color due to lack of contrast. For instance, if the background is black, or any color besides white, the system would have a difficult time distinguishing the pattern from the background, and possible not capture them at all. In addition, other problems arise from dirt and debris tracked onto the white surface of the floor, resulting in false patterns, further throwing the system.<br />
<br />
A solution is to substitute the black dots with LEDs placed atop the e-pucks, allowing the machine vision system to capture the identification pattern clearly regardless of background color and condition. By adjusting the threshold set in the machine vision code, the system will rely on the contrast of light intensity, minimizing the interference of the operating environment whose light intensity is which is naturally weaker than LEDs'. <br />
====Change from Original Code====<br />
In '''main.cpp''' in '''RGBVisionTracking.vcproj''', the RGB Vision project, the code has been changed in<br />
<br />
'''Line 56''':<br />
cvThreshold(greyImage[camerai], thresholdedImage[camerai], threshold, 255, '''CV_THRESH_BINARY_INV''');<br />
to<br />
cvThreshold(greyImage[camerai], thresholdedImage[camerai], threshold, 255, '''CV_THRESH_BINARY''');<br />
<br />
and<br />
<br />
'''Line 731''':<br />
cvThreshold(grey, thresholded_image, threshold, 255, '''CV_THRESH_BINARY_INV''');<br />
to<br />
cvThreshold(grey, thresholded_image, threshold, 255, '''CV_THRESH_BINARY''');<br />
<br />
Also, in '''global_vars.h''',<br />
<br />
'''Line 65''':<br />
double threshold = '''75'''; //black/white threshold<br />
to<br />
double threshold = '''200'''; //black/white threshold<br />
<br />
As change '''''CV_THRESH_BINARY_INV''''' in both line 48 and 735 to '''''CV_THRESH_BINARY''''' and adjust the value of threshold from '''''75''''' to '''''200''''', the system now clearly presents LED patterns as white dot patterns on the screen, so it can identify e-pucks according to LED patterns.<br />
<br />
====Threshold Testing====<br />
The threshold value of ''200'' is determined to be good enough for the test inside. With various conditions, however, the threshold value can, or should, be changed more properly. In addition, the results for different range of threshold under the same test condition is presented below:<br />
{| class="wikitable" border="3"<br />
|+'''Threshold range'''<br />
|-<br />
! Range !! Result <br />
|-<br />
| 0 - 94 || System cannot caputure LED patterns at all; whole screen is white.<br />
|-<br />
| 95 - 170 || System can recognize the pattern but it is unstable, since most of background becomes white.<br />
|-<br />
| 171 - 252 || System cleary captures and recognizes LED patterns.<br />
|-<br />
| 253|| System can recognize the pattern but it is unstable since pattern is too small; stronger intensity is required.<br />
|-<br />
| 254 - 255 || System cannot caputure LED patterns at all; whole screen is black.<br />
|}<br />
<br />
<br />
An e-puck was fitted with a LED pattern board and then tested with the machine vision localization system. With the changes implemented, the machine vision localization system did not show any problems, showing the ability to capture and locate the e-puck located in anywhere in the field of vision of the cameras. In addition, the vision system was able to capture and locate the e-puck as it moved. There was no loss of positional accuracy as compared to previous implementations of identification systems. The recognition of the e-puck by the machine vision localization system displayed the stability of the LED boards with the vision system, further supporting their implementation for further experiments.<br />
<br />
===Center of Mass Problem with LEDs===<br />
Another problem with the implementation of the LED pattern boards is related to the method that the machine vision localization system generates a position of a puck. Originally, the vision system determines the center of the mass of the paper dice dot patterns ('''R<sub>dots</sub>'''), and calculates the world position coordinate using the calibration information. From this, the vision system then assigns '''R<sub>dots</sub>''' as the center of mass of the e-puck ('''R<sub>e-puck</sub>'''). While '''R<sub>dots</sub>''' is typically not located over '''R<sub>e-puck</sub>''', the paper dots location was shifted in order to match '''R<sub>dots</sub>''' with '''R<sub>e-puck</sub>'''.<br />
<br />
However, this is not achievable with the LED pattern boards. As LED pattern board's location on the e-puck cannot be shifted, the vision system needs to be able to shift '''R<sub>LEDs</sub>''' to accommodate for this position error. By augmenting the '''target_classifiers.txt''' file with additional data, the modified vision system refers to a look-up table in order to determine the amount and direction that the coordinates of center of mass of the LED dots ('''R<sub>LEDs</sub>''') needs to be shifted to align with '''R<sub>e-puck</sub>'''. The look-up table contains values that are unique and constant to each rotationally invariant pattern, and as a result, only a few simple calculations are needed to generate correct coordinates for the e-puck.<br />
<br />
The result is the vision system yields a much more accurate position data for the e-puck. A simple test, which consists of rotating the e-puck around the '''R<sub>e-puck</sub>''' shows that the coordinate data for '''R<sub>e-puck</sub>''' varies by roughly 4-5mm when using the RGB modified code. Using the original Machine Vision Localization System code, the coordinate data for '''R<sub>e-puck</sub>''' varies by much more, between 40-50mm. Note that the only difference between these two tests, is the application of code to shift the '''R<sub>e-puck</sub>''' back into place; otherwise both sets of code are changed to accommodate the LED pattern boards, which were used in this test.<br />
<br />
====Addition to the Original Code====<br />
There are only a few minor changes in the code from the original machine vision code to the RGB machine vision code. The change focused around the introduction of '''centermag''' and '''centeroffset''', two new variables. '''Centermag''' refers to the magnitude from the vision system calculated '''R<sub>dots</sub>''' to the desired '''R<sub>e-puck</sub>'''. This value differs from e-puck to e-puck, the the value remains constant for each e-puck regardless of orientation. '''Centeroffset''' refers to the angle offset between the vision system calculated angle, and the line from '''R<sub>dots</sub>''' to the desired '''R<sub>e-puck</sub>'''. Again, this value differs from e-puck to e-puck, and also remains constant for each e-puck regardless of orientation.<br />
<br />
The actual code which shifts the coordinate data from '''R<sub>dots</sub>''' to the desired '''R<sub>e-puck</sub>''' is simple.<br />
<br />
In '''main.cpp''' of '''RGBVisionTracking.proj''', the code is as follows:<br />
<br />
line '''1082''' through line '''1109'''<br />
targets_temp[camerai]->group.wx = targets_temp[camerai]->group.wx + -1*targets_temp[camerai]->centermag*cos(Adjusted);<br />
targets_temp[camerai]->group.wy = targets_temp[camerai]->group.wy + -1*targets_temp[camerai]->centermag*sin(Adjusted);<br />
<br />
'''Adjusted''' is the center offset angle plus the orientation angle, or '''centeroffset + angle''', which results is the angle of the line from '''R<sub>dots</sub>''' to '''R<sub>e-puck</sub>''', from zero degrees. With knowledge of this angle, a simply trigonometry problem is performed to determine the x and y values to add/subtract from '''R<sub>dots</sub>''' to get coordinates for '''R<sub>e-puck</sub>'''.<br />
<br />
The values for centermag and centeroffset are found in the txt file, '''target_classifiers_augmented''', as the fourth and fifth values, respectively. This file can be round in the modified code zip file above.<br />
<br />
===Other Considerations===<br />
While there do not need to be any additional changes to the set up of the machine vision localization system, there may be additional considerations for further development. One such consideration is the 'background', or floor material, of the setup. With the modified machine vision code, light intensity is what is picked up and filtered by the system, thus rendering the LEDs from the e-pucks to be the only tracked objects. However, with more advanced set ups, such as one featuring light that is projected onto the background, this may present a problem with the machine vision system picking up reflected light. More testing has to be done with modifying the machine vision system threshold to see if there is an ideal threshold to accommodate this setup. Another option may be to use a non-reflective or matte surface for the background.<br />
<br />
Another consideration involves the hardware of the setup, or the themselves. The cameras are equipped with the Logitech software which automatically adjusts the exposure and light contrast settings to correct for poor lighting and setup conditions. However, this leads to issues as with increased exposure (due to light blocking set up) and bright LEDs results in blurry or blobby images received. The machine vision localization system cannot read these images, and as a result cannot track the e-pucks. One potential solution may be to adjust the threshold of the vision system. Other solutions may be to use LEDs with a lower millicandela, or increase the background lighting of the setup, for instance with upward casting lights.<br />
<br />
==RGB Swarm E-puck Code Overview==<br />
Detailed code outline: [[Swarm RGB E-puck Code]]<br />
<br />
==MATLAB Code for RGB==<br />
<br />
[[Media:RGB_Swarm_MATLAB_09-03-09.zip]]<br />
<br />
====What to do with the files:====<br />
* '''open_serial.m:''' Change the COM call to whatever COM port your XBee radio is plugged into (COM1, COM2, ...). Run this to open the COM port before you run anything else. It will create a COM object in your workspace. It takes a long time, be patient.<br />
* '''close_serial.m:''' Run after you're done to close the COM port. Keeps matlab and your computer happy.<br />
* '''RGB_Swarm_Data_Grabber.m:''' Use this to plot the values from the RGB sensor of one puck. It shows a moving plot. Handy for testing.<br />
* '''RGB_avg_std_logger.m:''' Interactive point logger. Follow the prompts to grab some nice data. Useful for testing.<br />
* '''color_hist.m:''' Plot a histogram from RGB_Swarm_Data_Grabber.<br />
<br />
==Physical Setup==<br />
In the RGB swarm robot project, the epucks pick up light from a projector. This projector has to project onto the floor so that the top mounted light sensors can pick up the light. The floor which the epucks roll on must be completely enclosed so that the only light which reaches it, is the light from the projector. Also this floor must be smooth, flat and durable. See the overhead view below.<br />
<br />
{| align="left" cellpadding = "25" <br />
! [[Image:RGBswarmsetup.jpg|600px|center]]<br />
|}<br />
<br />
<br clear=all><br />
<br />
===Curtains===<br />
The floor is enclosed by two walls and 6 curtains. Two bars protrude from the walls and are connected by an L-joint. There are 3 Eclipse absolute zero curtains on each bar (see diagram). These curtains block 100% light and are sewn together so that no light comes through between them. Covering the whole enclosure, above the projector mount are 7 more curtains sewn together to block all light. <br />
<br />
<br />
<br />
===Floor===<br />
The floor is currently a vinyl sheet from home depot. The sheet had a raised pattern on the front, so it was flipped over. The underside of the vinyl floor is not very durable, but holds up as long as no shoes are used when stepping on it. In the future, a more durable light colored floor may want to be used. Also, the floor is taped down with packing tape. The tape should not be removed as the floor has markings which show where the camera calibration dots should go and where the projected image should fall.<br />
<br />
<br />
<br />
===Projector===<br />
The projector is a Benq MP771 DLP prpjector. It has a digital user manual on a CD in the projection computer. <br />
<br />
Since it is DLP, it has an array of tiny mirrors which reflect the light from the bulb. The light from the bulb is shown through a color wheel which shines red, green, and blue on to the mirror array. The frequency with which the mirrors turn on and off (reflect light and don't) determines the intensity of light. For example if a dark red was being projected, the mirrors would be on more than off in a certain interval. In the case of our projector that interval is %%??8.2 millisecond??%%.<br />
<br />
Each mirror represents a different pixel projected from the projector. This projector has a resolution of ?1024 x 760?, so in order to get a 1 to 1 pixel ratio, the projection computer should be set to display at 1024 x 760. <br />
<br />
The projector should not be tilted forward or backward more that 15°. Because of this and the wide throw of the projector, a keystone projection shape could not be avoided. The projector is programmed to project at the maximum amount of keystone.<br />
<br />
The size of the projected image....<br />
<br />
====Projector PWM Waveform====<br />
{|<br />
| [[Image:Projector-waveform-longtime.jpg|200px|thumb|alt=Waveform from the color sensor under projector light (long timescale)|Waveform from the color sensor under projector light (long timescale)]]<br />
| [[Image:Red-high-value.jpg|200px|thumb|alt=Waveform from the color sensor under projected high value red|Waveform from the color sensor under projected high value red]]<br />
| [[Image:Red-med-value.jpg|200px|thumb|alt=Waveform from the color sensor under projected medium value red|Waveform from the color sensor under projected medium value red]]<br />
| [[Image:Red-low-value.jpg|200px|thumb|alt=Waveform from the color sensor under projected low value red|Waveform from the color sensor under projected low value red]]<br />
| <br />
|}<br />
The projector pulse width modulates the color output. So you need to average the measured intensity over the period of the projector to measure the color. The period of the projector is about 8ms.<br />
<br />
===Projector Mount===<br />
Etiam a odio sit amet nunc ullamcorper fringilla non id dui. Sed interdum, ligula pretium aliquet ornare, ligula metus sodales nisi, in fermentum erat purus id diam. Suspendisse tincidunt scelerisque enim, vitae hendrerit nunc suscipit ac. Cras luctus, nisl eu congue sagittis, nibh quam ultricies eros, vitae ultricies odio mauris vitae urna. Vestibulum ante ipsum primis in faucibus orci luctus et ultrices posuere cubilia Curae; Nam in felis cursus urna pretium scelerisque. Morbi lacinia semper venenatis. Integer pharetra risus et purus luctus pellentesque. Nam risus justo, consectetur nec vestibulum eu, molestie eget felis. Sed tristique congue nulla eget luctus. In suscipit dignissim mollis. Morbi ac pellentesque lacus. Sed vel mauris at nulla imperdiet hendrerit id et sem. Sed elementum semper quam sed feugiat. Suspendisse quis felis vitae sapien placerat aliquet nec quis mauris. Vivamus eget auctor diam. Suspendisse convallis leo nec turpis iaculis quis fringilla libero hendrerit.<br />
<br />
=='''Conclusion'''==<br />
<br />
The new XBee Interface Extension Board design was tested, and we found out that it does not have any problem. In addition, the black dot pattern of the e-pucks are upgraded to LED patterns. The advantage of this improvement is that the machine vision system can recoginize each e-puck no matter where the e-pucks are located. The color of the background also does not affect the vision system. However, we had to move the color sensor to the LED pattern board since the LED pattern board will block the sensor if the sensor is located in the XBee Interface Extension Board. Thus, we now consider the light interference between the LEDs and the color sensor. In the light interference test, we found out that the color sensor is affected by the light from LED. However, since we used much brighter LEDs in our light interference test than the LEDs used for the LED pattern board, we have to do more experiment on this in order to have more accurate interference data. <br />
<br />
<br />
[[Category:SwarmRobotProject]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=File:Red-med-value.jpg&diff=14284File:Red-med-value.jpg2009-09-04T18:03:16Z<p>Sam Bobb: </p>
<hr />
<div></div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=File:Red-low-value.jpg&diff=14283File:Red-low-value.jpg2009-09-04T18:03:01Z<p>Sam Bobb: </p>
<hr />
<div></div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=File:Red-high-value.jpg&diff=14282File:Red-high-value.jpg2009-09-04T18:02:45Z<p>Sam Bobb: </p>
<hr />
<div></div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=File:Projector-waveform-longtime.jpg&diff=14281File:Projector-waveform-longtime.jpg2009-09-04T18:02:09Z<p>Sam Bobb: </p>
<hr />
<div></div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_E-puck_Code_(outdated)&diff=14280RGB Swarm Robot Project E-puck Code (outdated)2009-09-04T17:58:38Z<p>Sam Bobb: /* Complete */</p>
<hr />
<div>This page documents the e-puck code for the RGB Sensing Swarm Robotics project. The code on the e-puck was written in C and compiled using Microchip's MPLAB C Compiler for dsPIC DSCs (student version). <br />
<br />
This code is a branch of the [[Swarm Project E-puck Code]].<br />
<br />
==Tasks==<br />
<br />
===Complete===<br />
* Got ADC working for color sensor<br />
** Set up filtering to address projector PWM<br />
* Restructured code to make more modular.<br />
** Split dsPIC_XBeePackets and wheel_speed_coordinator into h and c files<br />
** Pulled packet assembling code out of main and created send_packet() function in send_packet.h/c.<br />
** Pulled a bunch of variables and defines (''NUM_DATA_SETS, NUMBERS_PER_SET, DATATYPE_BYTELENGTH , DATA_ARRAY_LENGTH , ADDITIONAL_NUMS, notRTS, T1_INT_FLAG, x_i, u_i, w_i, x_sum, w_sum, MAX_WHEEL_V_TICKS, deadband, COMMR, SAFEDIST, MINDIST, u_x_ideal, u_y_ideal, x_motion_integral, y_motion_integral, SQUARE'') that were scattered across h files into global_vars.h/c. Makes it easy to include them in a particular file with the ''extern'' keyword.<br />
* Added color_cal() function in color_cal.h/c<br />
* Added wheelSpeedSingleBot to wheel_speed_coordinator<br />
<br />
===To Do===<br />
* Finish color_cal<br />
* Make the vision system position information updater smarter.<br />
* Replace wheelSpeedSingleBot with the three step move controller from NUtest.c<br />
* Implement new algorithm<br />
<br />
<br />
==Project Package==<br />
The source code for the project is available here: [[Media:RGB_Swarm_Puck_Code_working_version.zip]]. Open swarm_epucks.mcw and you should be good to go.<br />
<br />
==Description of the files and functions==<br />
<br />
===global_vars(.c/.h)===<br />
* .c/.h: declare and define global variables and macros<br />
====Packet Length Constants====<br />
These variables determine the length of the XBee packets. See [[Swarm_Robot_Project_Documentation#Data_Frame|Data Frame]] and the section on XBee API packets in the XBee manual for further clarification.<br />
<br />
Much of this is still sending integral consensus estimator data. This can be removed or replaces with data needed for decentralized color sensing. <br />
<br />
=====#define NUM_DATA_SETS 5 ===== <br />
Number of statistics on which you are running the consensus estimator. This this particular case, 5. (Ix, Iy, Ixx, Ixy, Iyy)<br />
<br />
=====#define NUMBERS_PER_SET 2=====<br />
Number of variables in each data set (see above) that the consensus estimator needs to transmit to other agents. In this case, 2 because there is <tt>x_i</tt> and <tt>w_i</tt> for each statistic.<br />
<br />
=====#define DATATYPE_BYTELENGTH 4 =====<br />
Number of bytes in the data type (used in the consensus estimator (float = 4 bytes long). This is important because we need to split the numbers into individual bytes to be able to send them out the serial port.<br />
<br />
=====#define DATA_ARRAY_LENGTH (NUM_DATA_SETS*NUMBERS_PER_SET)=====<br />
Total number of data variables needed for the consensus estimator. In this case, it is 5*2=10.<br />
<br />
=====#define ADDITIONAL_NUMS 8=====<br />
Additional number of data to be appended to data array. It is 5 in this case, so that we can append <br />
# Robot X coordinate<br />
# Robot Y coordinate<br />
# Robot Theta orientation<br />
# Robot left wheel speed<br />
# Robot right wheel speed<br />
# Sensor Red Value<br />
# Sensor Green Value<br />
# Sensor Blue Value<br />
<br />
===main(.c/.h)===<br />
* .c: This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines. <br />
* .h: Contains variables, function prototypes, and delay function needed for main.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void)=====<br />
Timer1 ISR. Sets T1_INT_FLAG which provides timing for the main loop.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)=====<br />
UART2 receive ISR. Runs with the XBee receives packets. The switch/case structure handles the packets.<br />
<br />
The current handling of coordinate data from the vision system: <br />
<br />
<code><pre><br />
case 0: //coordinate data<br />
e_set_configuration(<br />
InPacket.data[1].dataFloat,<br />
InPacket.data[2].dataFloat, <br />
InPacket.data[3].dataFloat);<br />
break;<br />
</pre></code><br />
<br />
Needs to be improved. Suggested new function:<br />
<br />
* Get vision system data<br />
* Check if it's wildly off from current puck estimate<br />
** If not, update, clear log of rejected packets<br />
** If so, reject and log<br />
*** If we have rejected enough packets (some threshold) and they've all been in similar place (within tolerance), we can assume the puck is wrong and vision system is right. In this case, update with vision system data and clear rejected data log.<br />
<br />
=====int main(void)=====<br />
Setup functions and main loop.<br />
<br />
===color_cal(.c/.h)===<br />
* .c: Contains void calibrate_color(void) function to run the calibration routine.<br />
* .h: Contains function prototype and constant definitions for calibrate_color.<br />
<br />
=====void calibrate_color(void)=====<br />
This function runs the color calibration routine. Eventually this should be turned into a separate e-puck command from the vision system. Currently it just runs the puck through a zig zag pattern on the floor and sends packets. You can collect these with the data logger and process them in matlab. From this, you should be able to get a calibration function.<br />
<br />
'''To do:'''<br />
* Add to the pattern. Need to collect more data.<br />
* Possibly have the puck store data, find a best fit, and create the calibration function on board.<br />
* Store the calibration function in the EEPROM (flash memory) so it's non-volatile. The coefficients of function will be specific to each puck, so it would be nice to not have to program each puck, each time you change the battery.<br />
<br />
===dsPIC_XBeePackets(.c/.h)===<br />
* .c/.h: Contains functions and data structures for assembling and receiving XBee packets.<br />
<br />
=====int readPacket( void)=====<br />
Parses XBee data from UART2. Makes it accessable in the ''InPacket'' struct.<br />
<br />
The UART2 receive ISR takes error codes from this function and flashes the corresponding puck LED (absolute value of the error code).<br />
<br />
Error codes are:<br />
<br />
<code><pre><br />
//////////////ERROR CODES/////////////////<br />
// <br />
// -1: Timeout waiting for UART2<br />
// -2: Start delimiter wrong<br />
// -3: Checksum Error<br />
// -4: UART2 not ready at beginning<br />
// -5: API_ID unidentified<br />
// -6: Send Packet CCA failuue<br />
// -7: Modem Status packet failure<br />
//////////////////////////////////////////<br />
</pre></code><br />
<br />
===send_packet(.c/.h)===<br />
* .c: Contains the void send_packet(void) function which fills an array with data and calls the needed XBee functions to send a packet.<br />
* .h: Contains function prototype.<br />
<br />
=====void send_packet(void)=====<br />
* Creates ''packet'' array.<br />
* Adds consensus estimator data to the array.<br />
* Adds robot statistics to the array.<br />
* Adds color sensor values to the array.<br />
* Asserts flow control line to stop XBee from sending<br />
* Calls assemblePacket to send the packet.<br />
* Deasserts flow control; Xbee can send again.<br />
<br />
<br />
The function e_get_acc_filtered returns a running average of the acc specified. Syntax is:<br />
<br />
return value = e_get_acc_filtered(acc_channel, number of samples to average)<br />
<br />
The number of samples averaged must be less than ACC_SAMP_NB as defined in e_ad_conv.h.<br />
<br />
<code><pre><br />
packet[i]=e_get_acc_filtered(2, 136); // red<br />
i++;<br />
packet[i]=e_get_acc_filtered(1, 136); // green<br />
i++;<br />
packet[i]=e_get_acc_filtered(0, 136); // blue<br />
</pre></code><br />
<br />
===PI_consensus_estimator(.h)===<br />
* .h: Contains functions and data structures for the PI consensus estimator. <br />
* This will probably be replaced by the algorithm for sensor consensus. <br />
<br />
===wheel_speed_coordinator(.c/.h)===<br />
* .c: Contains functions for robot motion control<br />
* .h: Function prototypes and variabls.<br />
<br />
=====void wheelSpeed(int *vL, int *vR)=====<br />
Return needed wheel speeds for the inertial consensus estimator based on the group goal.<br />
<br />
=====void wheelSpeedSingleBot(float gotox, float gotoy, int *vL, int *vR)=====<br />
Return needed wheel speed to get this individual bot to (gotox, gotoy). It's a hacked fix. Should be replaced with the 3 step motion controller from NUtest.c.<br />
<br />
===e_acc(.c/.h)===<br />
* .c: Functions for reading the accelerometers (which is the color sensor).<br />
* .h: Function prototypes.<br />
<br />
This is original e-puck library code with the following modifications:<br />
<br />
<code><pre><br />
//changed by Sam, July 10, default offset is 2000. we want 0 for RGB sensor.<br />
static int centre_z = 0; //zero value for z axe<br />
</pre></code><br />
<br />
=====int e_get_acc_filtered(unsigned int captor, unsigned int filter_size)=====<br />
<br />
===e_ad_conv(.c/.h)===<br />
Set up the ADCs on the puck. Original e-puck library code, with the following modifications<br />
* .h: Define constants and functional prototypes<br />
<br />
MIC_SAMP_FREQ sets the baseline sampling frequency for the ADC, everything else must be a fraction of this. 16384 Hz is the highest possible.<br />
<code><pre><br />
#define MIC_SAMP_FREQ 16384.0 <br />
</pre></code><br />
<br />
ACC_PROX_SAMP_FREQ sets the sampling frequency for the accelerometers (color sensor). We found in testing that the puck become non-responsive with this set to 8192 Hz or 16384 Hz.<br />
<code><pre><br />
// sampling frequency for the accelerometres and proximetres<br />
//#define ACC_PROX_SAMP_FREQ 256.0 // WARNING: should be a fraction of MIC_SAMP_FREQ<br />
#define ACC_PROX_SAMP_FREQ 4096 // to ensure a good timing precision<br />
// So your options are: 1 2 4 8 16 32 64 128 <br />
// 256 512 1024 2048 4096 8192 16384<br />
</pre></code><br />
<br />
ACC_SAMP_NB is the number of samples to store. We can do an average of ''up to'' this many samples. This is set to 140 so we can average 136 samples, which is 4 projector periods.<br />
<code><pre><br />
#define ACC_SAMP_NB 140 // number of accelerometer samples to store<br />
</pre></code><br />
<br />
* .c: Functions and interrupt service routines for ADCs. Original e-puck library, no modifications.<br />
<br />
=====e_init_ad_scan(ALL_ADC)=====<br />
Call to setup ADC and have it work in the background. Use e_acc functions to access data.<br />
<br />
===e_init_port(.c/.h)===<br />
* .c/.h: Initializes the ports on the e-puck. File is from the standard e-puck library. <br />
<br />
=====e_init_ports(void)=====<br />
This function sets up ports on the e-puck. Call before using any ports.<br />
<br />
===e_led(.c/.h)===<br />
* .c/.h: This is a standard e-puck library file that contains functions for manipulating LEDs.<br />
<br />
=====void e_set_led(unsigned int led_number, unsigned int value)=====<br />
Set led_number (0-7) to value (0=off 1=on higher=inverse).<br />
<br />
[[Image:e-puck_LED_numbering.png|thumb|left]]<br />
<br />
<br clear='all'><br />
<br />
===e_motors_swarm(.c/.h)===<br />
* .c/.h: This file is a modified version of the e_motors.h e-puck library file. This version keeps track of the robot's position and orientation, and the motor stepping function contains code to update the robot's position when the wheels turn. <br />
* The functions like e_rotate and e_translate have been removed and this version is not dependent on e_agenda. <br />
<br />
=====#define POINT_OFFSET -31.75 =====<br />
In addition to constants like wheel radius and wheel base, the offset between the center point of the bot and the point we're driving is set in e_motors_swarm.h. -31.75 mm is the distance betwene the center and the color sensor. So technically, we're driving backwards.<br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T5Interrupt(void)=====<br />
Timer5 ISR, interrupt for left motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos + cval*LINSTEP;<br />
ypos = ypos + sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T4Interrupt(void)=====<br />
Timer4 ISR, interrupt for right motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos - cval*LINSTEP;<br />
ypos = ypos - sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void e_init_motors(void)=====<br />
Call this function before other motor functions to initialize the motors.<br />
<br />
=====void e_set_speed_left(int motor_speed)/void e_set_speed_right(int motor_speed)=====<br />
Set the motor speed in steps/second.<br />
<br />
=====void e_get_configuration(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the center reference point.<br />
<br />
=====void e_set_configuration(float x, float y, float theta)=====<br />
Sets x, y, theta to values. This is used to overwrite the puck position/orientation estimates with data from the vision system.<br />
<br />
=====void e_get_configuration_front(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the front reference point (used for motor control).<br />
<br />
<br />
[[Category:SwarmRobotProject]]<br />
[[Category:e-puck]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=File:RGB_Swarm_Puck_Code_working_version.zip&diff=14279File:RGB Swarm Puck Code working version.zip2009-09-04T17:53:40Z<p>Sam Bobb: uploaded a new version of "Image:RGB Swarm Puck Code working version.zip"</p>
<hr />
<div></div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_E-puck_Code_(outdated)&diff=14278RGB Swarm Robot Project E-puck Code (outdated)2009-09-04T17:51:54Z<p>Sam Bobb: /* #define POINT_OFFSET -31.75 */</p>
<hr />
<div>This page documents the e-puck code for the RGB Sensing Swarm Robotics project. The code on the e-puck was written in C and compiled using Microchip's MPLAB C Compiler for dsPIC DSCs (student version). <br />
<br />
This code is a branch of the [[Swarm Project E-puck Code]].<br />
<br />
==Tasks==<br />
<br />
===Complete===<br />
* Restructured code to make more modular.<br />
** Split dsPIC_XBeePackets and wheel_speed_coordinator into h and c files<br />
** Pulled packet assembling code out of main and created send_packet() function in send_packet.h/c.<br />
** Pulled a bunch of variables and defines (''NUM_DATA_SETS, NUMBERS_PER_SET, DATATYPE_BYTELENGTH , DATA_ARRAY_LENGTH , ADDITIONAL_NUMS, notRTS, T1_INT_FLAG, x_i, u_i, w_i, x_sum, w_sum, MAX_WHEEL_V_TICKS, deadband, COMMR, SAFEDIST, MINDIST, u_x_ideal, u_y_ideal, x_motion_integral, y_motion_integral, SQUARE'') that were scattered across h files into global_vars.h/c. Makes it easy to include them in a particular file with the ''extern'' keyword.<br />
* Added color_cal() function in color_cal.h/c<br />
* Added wheelSpeedSingleBot to wheel_speed_coordinator<br />
<br />
===To Do===<br />
* Finish color_cal<br />
* Make the vision system position information updater smarter.<br />
* Replace wheelSpeedSingleBot with the three step move controller from NUtest.c<br />
* Implement new algorithm<br />
<br />
<br />
==Project Package==<br />
The source code for the project is available here: [[Media:RGB_Swarm_Puck_Code_working_version.zip]]. Open swarm_epucks.mcw and you should be good to go.<br />
<br />
==Description of the files and functions==<br />
<br />
===global_vars(.c/.h)===<br />
* .c/.h: declare and define global variables and macros<br />
====Packet Length Constants====<br />
These variables determine the length of the XBee packets. See [[Swarm_Robot_Project_Documentation#Data_Frame|Data Frame]] and the section on XBee API packets in the XBee manual for further clarification.<br />
<br />
Much of this is still sending integral consensus estimator data. This can be removed or replaces with data needed for decentralized color sensing. <br />
<br />
=====#define NUM_DATA_SETS 5 ===== <br />
Number of statistics on which you are running the consensus estimator. This this particular case, 5. (Ix, Iy, Ixx, Ixy, Iyy)<br />
<br />
=====#define NUMBERS_PER_SET 2=====<br />
Number of variables in each data set (see above) that the consensus estimator needs to transmit to other agents. In this case, 2 because there is <tt>x_i</tt> and <tt>w_i</tt> for each statistic.<br />
<br />
=====#define DATATYPE_BYTELENGTH 4 =====<br />
Number of bytes in the data type (used in the consensus estimator (float = 4 bytes long). This is important because we need to split the numbers into individual bytes to be able to send them out the serial port.<br />
<br />
=====#define DATA_ARRAY_LENGTH (NUM_DATA_SETS*NUMBERS_PER_SET)=====<br />
Total number of data variables needed for the consensus estimator. In this case, it is 5*2=10.<br />
<br />
=====#define ADDITIONAL_NUMS 8=====<br />
Additional number of data to be appended to data array. It is 5 in this case, so that we can append <br />
# Robot X coordinate<br />
# Robot Y coordinate<br />
# Robot Theta orientation<br />
# Robot left wheel speed<br />
# Robot right wheel speed<br />
# Sensor Red Value<br />
# Sensor Green Value<br />
# Sensor Blue Value<br />
<br />
===main(.c/.h)===<br />
* .c: This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines. <br />
* .h: Contains variables, function prototypes, and delay function needed for main.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void)=====<br />
Timer1 ISR. Sets T1_INT_FLAG which provides timing for the main loop.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)=====<br />
UART2 receive ISR. Runs with the XBee receives packets. The switch/case structure handles the packets.<br />
<br />
The current handling of coordinate data from the vision system: <br />
<br />
<code><pre><br />
case 0: //coordinate data<br />
e_set_configuration(<br />
InPacket.data[1].dataFloat,<br />
InPacket.data[2].dataFloat, <br />
InPacket.data[3].dataFloat);<br />
break;<br />
</pre></code><br />
<br />
Needs to be improved. Suggested new function:<br />
<br />
* Get vision system data<br />
* Check if it's wildly off from current puck estimate<br />
** If not, update, clear log of rejected packets<br />
** If so, reject and log<br />
*** If we have rejected enough packets (some threshold) and they've all been in similar place (within tolerance), we can assume the puck is wrong and vision system is right. In this case, update with vision system data and clear rejected data log.<br />
<br />
=====int main(void)=====<br />
Setup functions and main loop.<br />
<br />
===color_cal(.c/.h)===<br />
* .c: Contains void calibrate_color(void) function to run the calibration routine.<br />
* .h: Contains function prototype and constant definitions for calibrate_color.<br />
<br />
=====void calibrate_color(void)=====<br />
This function runs the color calibration routine. Eventually this should be turned into a separate e-puck command from the vision system. Currently it just runs the puck through a zig zag pattern on the floor and sends packets. You can collect these with the data logger and process them in matlab. From this, you should be able to get a calibration function.<br />
<br />
'''To do:'''<br />
* Add to the pattern. Need to collect more data.<br />
* Possibly have the puck store data, find a best fit, and create the calibration function on board.<br />
* Store the calibration function in the EEPROM (flash memory) so it's non-volatile. The coefficients of function will be specific to each puck, so it would be nice to not have to program each puck, each time you change the battery.<br />
<br />
===dsPIC_XBeePackets(.c/.h)===<br />
* .c/.h: Contains functions and data structures for assembling and receiving XBee packets.<br />
<br />
=====int readPacket( void)=====<br />
Parses XBee data from UART2. Makes it accessable in the ''InPacket'' struct.<br />
<br />
The UART2 receive ISR takes error codes from this function and flashes the corresponding puck LED (absolute value of the error code).<br />
<br />
Error codes are:<br />
<br />
<code><pre><br />
//////////////ERROR CODES/////////////////<br />
// <br />
// -1: Timeout waiting for UART2<br />
// -2: Start delimiter wrong<br />
// -3: Checksum Error<br />
// -4: UART2 not ready at beginning<br />
// -5: API_ID unidentified<br />
// -6: Send Packet CCA failuue<br />
// -7: Modem Status packet failure<br />
//////////////////////////////////////////<br />
</pre></code><br />
<br />
===send_packet(.c/.h)===<br />
* .c: Contains the void send_packet(void) function which fills an array with data and calls the needed XBee functions to send a packet.<br />
* .h: Contains function prototype.<br />
<br />
=====void send_packet(void)=====<br />
* Creates ''packet'' array.<br />
* Adds consensus estimator data to the array.<br />
* Adds robot statistics to the array.<br />
* Adds color sensor values to the array.<br />
* Asserts flow control line to stop XBee from sending<br />
* Calls assemblePacket to send the packet.<br />
* Deasserts flow control; Xbee can send again.<br />
<br />
<br />
The function e_get_acc_filtered returns a running average of the acc specified. Syntax is:<br />
<br />
return value = e_get_acc_filtered(acc_channel, number of samples to average)<br />
<br />
The number of samples averaged must be less than ACC_SAMP_NB as defined in e_ad_conv.h.<br />
<br />
<code><pre><br />
packet[i]=e_get_acc_filtered(2, 136); // red<br />
i++;<br />
packet[i]=e_get_acc_filtered(1, 136); // green<br />
i++;<br />
packet[i]=e_get_acc_filtered(0, 136); // blue<br />
</pre></code><br />
<br />
===PI_consensus_estimator(.h)===<br />
* .h: Contains functions and data structures for the PI consensus estimator. <br />
* This will probably be replaced by the algorithm for sensor consensus. <br />
<br />
===wheel_speed_coordinator(.c/.h)===<br />
* .c: Contains functions for robot motion control<br />
* .h: Function prototypes and variabls.<br />
<br />
=====void wheelSpeed(int *vL, int *vR)=====<br />
Return needed wheel speeds for the inertial consensus estimator based on the group goal.<br />
<br />
=====void wheelSpeedSingleBot(float gotox, float gotoy, int *vL, int *vR)=====<br />
Return needed wheel speed to get this individual bot to (gotox, gotoy). It's a hacked fix. Should be replaced with the 3 step motion controller from NUtest.c.<br />
<br />
===e_acc(.c/.h)===<br />
* .c: Functions for reading the accelerometers (which is the color sensor).<br />
* .h: Function prototypes.<br />
<br />
This is original e-puck library code with the following modifications:<br />
<br />
<code><pre><br />
//changed by Sam, July 10, default offset is 2000. we want 0 for RGB sensor.<br />
static int centre_z = 0; //zero value for z axe<br />
</pre></code><br />
<br />
=====int e_get_acc_filtered(unsigned int captor, unsigned int filter_size)=====<br />
<br />
===e_ad_conv(.c/.h)===<br />
Set up the ADCs on the puck. Original e-puck library code, with the following modifications<br />
* .h: Define constants and functional prototypes<br />
<br />
MIC_SAMP_FREQ sets the baseline sampling frequency for the ADC, everything else must be a fraction of this. 16384 Hz is the highest possible.<br />
<code><pre><br />
#define MIC_SAMP_FREQ 16384.0 <br />
</pre></code><br />
<br />
ACC_PROX_SAMP_FREQ sets the sampling frequency for the accelerometers (color sensor). We found in testing that the puck become non-responsive with this set to 8192 Hz or 16384 Hz.<br />
<code><pre><br />
// sampling frequency for the accelerometres and proximetres<br />
//#define ACC_PROX_SAMP_FREQ 256.0 // WARNING: should be a fraction of MIC_SAMP_FREQ<br />
#define ACC_PROX_SAMP_FREQ 4096 // to ensure a good timing precision<br />
// So your options are: 1 2 4 8 16 32 64 128 <br />
// 256 512 1024 2048 4096 8192 16384<br />
</pre></code><br />
<br />
ACC_SAMP_NB is the number of samples to store. We can do an average of ''up to'' this many samples. This is set to 140 so we can average 136 samples, which is 4 projector periods.<br />
<code><pre><br />
#define ACC_SAMP_NB 140 // number of accelerometer samples to store<br />
</pre></code><br />
<br />
* .c: Functions and interrupt service routines for ADCs. Original e-puck library, no modifications.<br />
<br />
=====e_init_ad_scan(ALL_ADC)=====<br />
Call to setup ADC and have it work in the background. Use e_acc functions to access data.<br />
<br />
===e_init_port(.c/.h)===<br />
* .c/.h: Initializes the ports on the e-puck. File is from the standard e-puck library. <br />
<br />
=====e_init_ports(void)=====<br />
This function sets up ports on the e-puck. Call before using any ports.<br />
<br />
===e_led(.c/.h)===<br />
* .c/.h: This is a standard e-puck library file that contains functions for manipulating LEDs.<br />
<br />
=====void e_set_led(unsigned int led_number, unsigned int value)=====<br />
Set led_number (0-7) to value (0=off 1=on higher=inverse).<br />
<br />
[[Image:e-puck_LED_numbering.png|thumb|left]]<br />
<br />
<br clear='all'><br />
<br />
===e_motors_swarm(.c/.h)===<br />
* .c/.h: This file is a modified version of the e_motors.h e-puck library file. This version keeps track of the robot's position and orientation, and the motor stepping function contains code to update the robot's position when the wheels turn. <br />
* The functions like e_rotate and e_translate have been removed and this version is not dependent on e_agenda. <br />
<br />
=====#define POINT_OFFSET -31.75 =====<br />
In addition to constants like wheel radius and wheel base, the offset between the center point of the bot and the point we're driving is set in e_motors_swarm.h. -31.75 mm is the distance betwene the center and the color sensor. So technically, we're driving backwards.<br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T5Interrupt(void)=====<br />
Timer5 ISR, interrupt for left motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos + cval*LINSTEP;<br />
ypos = ypos + sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T4Interrupt(void)=====<br />
Timer4 ISR, interrupt for right motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos - cval*LINSTEP;<br />
ypos = ypos - sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void e_init_motors(void)=====<br />
Call this function before other motor functions to initialize the motors.<br />
<br />
=====void e_set_speed_left(int motor_speed)/void e_set_speed_right(int motor_speed)=====<br />
Set the motor speed in steps/second.<br />
<br />
=====void e_get_configuration(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the center reference point.<br />
<br />
=====void e_set_configuration(float x, float y, float theta)=====<br />
Sets x, y, theta to values. This is used to overwrite the puck position/orientation estimates with data from the vision system.<br />
<br />
=====void e_get_configuration_front(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the front reference point (used for motor control).<br />
<br />
<br />
[[Category:SwarmRobotProject]]<br />
[[Category:e-puck]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_E-puck_Code_(outdated)&diff=14277RGB Swarm Robot Project E-puck Code (outdated)2009-09-04T17:51:45Z<p>Sam Bobb: /* e_motors_swarm(.c/.h) */</p>
<hr />
<div>This page documents the e-puck code for the RGB Sensing Swarm Robotics project. The code on the e-puck was written in C and compiled using Microchip's MPLAB C Compiler for dsPIC DSCs (student version). <br />
<br />
This code is a branch of the [[Swarm Project E-puck Code]].<br />
<br />
==Tasks==<br />
<br />
===Complete===<br />
* Restructured code to make more modular.<br />
** Split dsPIC_XBeePackets and wheel_speed_coordinator into h and c files<br />
** Pulled packet assembling code out of main and created send_packet() function in send_packet.h/c.<br />
** Pulled a bunch of variables and defines (''NUM_DATA_SETS, NUMBERS_PER_SET, DATATYPE_BYTELENGTH , DATA_ARRAY_LENGTH , ADDITIONAL_NUMS, notRTS, T1_INT_FLAG, x_i, u_i, w_i, x_sum, w_sum, MAX_WHEEL_V_TICKS, deadband, COMMR, SAFEDIST, MINDIST, u_x_ideal, u_y_ideal, x_motion_integral, y_motion_integral, SQUARE'') that were scattered across h files into global_vars.h/c. Makes it easy to include them in a particular file with the ''extern'' keyword.<br />
* Added color_cal() function in color_cal.h/c<br />
* Added wheelSpeedSingleBot to wheel_speed_coordinator<br />
<br />
===To Do===<br />
* Finish color_cal<br />
* Make the vision system position information updater smarter.<br />
* Replace wheelSpeedSingleBot with the three step move controller from NUtest.c<br />
* Implement new algorithm<br />
<br />
<br />
==Project Package==<br />
The source code for the project is available here: [[Media:RGB_Swarm_Puck_Code_working_version.zip]]. Open swarm_epucks.mcw and you should be good to go.<br />
<br />
==Description of the files and functions==<br />
<br />
===global_vars(.c/.h)===<br />
* .c/.h: declare and define global variables and macros<br />
====Packet Length Constants====<br />
These variables determine the length of the XBee packets. See [[Swarm_Robot_Project_Documentation#Data_Frame|Data Frame]] and the section on XBee API packets in the XBee manual for further clarification.<br />
<br />
Much of this is still sending integral consensus estimator data. This can be removed or replaces with data needed for decentralized color sensing. <br />
<br />
=====#define NUM_DATA_SETS 5 ===== <br />
Number of statistics on which you are running the consensus estimator. This this particular case, 5. (Ix, Iy, Ixx, Ixy, Iyy)<br />
<br />
=====#define NUMBERS_PER_SET 2=====<br />
Number of variables in each data set (see above) that the consensus estimator needs to transmit to other agents. In this case, 2 because there is <tt>x_i</tt> and <tt>w_i</tt> for each statistic.<br />
<br />
=====#define DATATYPE_BYTELENGTH 4 =====<br />
Number of bytes in the data type (used in the consensus estimator (float = 4 bytes long). This is important because we need to split the numbers into individual bytes to be able to send them out the serial port.<br />
<br />
=====#define DATA_ARRAY_LENGTH (NUM_DATA_SETS*NUMBERS_PER_SET)=====<br />
Total number of data variables needed for the consensus estimator. In this case, it is 5*2=10.<br />
<br />
=====#define ADDITIONAL_NUMS 8=====<br />
Additional number of data to be appended to data array. It is 5 in this case, so that we can append <br />
# Robot X coordinate<br />
# Robot Y coordinate<br />
# Robot Theta orientation<br />
# Robot left wheel speed<br />
# Robot right wheel speed<br />
# Sensor Red Value<br />
# Sensor Green Value<br />
# Sensor Blue Value<br />
<br />
===main(.c/.h)===<br />
* .c: This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines. <br />
* .h: Contains variables, function prototypes, and delay function needed for main.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void)=====<br />
Timer1 ISR. Sets T1_INT_FLAG which provides timing for the main loop.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)=====<br />
UART2 receive ISR. Runs with the XBee receives packets. The switch/case structure handles the packets.<br />
<br />
The current handling of coordinate data from the vision system: <br />
<br />
<code><pre><br />
case 0: //coordinate data<br />
e_set_configuration(<br />
InPacket.data[1].dataFloat,<br />
InPacket.data[2].dataFloat, <br />
InPacket.data[3].dataFloat);<br />
break;<br />
</pre></code><br />
<br />
Needs to be improved. Suggested new function:<br />
<br />
* Get vision system data<br />
* Check if it's wildly off from current puck estimate<br />
** If not, update, clear log of rejected packets<br />
** If so, reject and log<br />
*** If we have rejected enough packets (some threshold) and they've all been in similar place (within tolerance), we can assume the puck is wrong and vision system is right. In this case, update with vision system data and clear rejected data log.<br />
<br />
=====int main(void)=====<br />
Setup functions and main loop.<br />
<br />
===color_cal(.c/.h)===<br />
* .c: Contains void calibrate_color(void) function to run the calibration routine.<br />
* .h: Contains function prototype and constant definitions for calibrate_color.<br />
<br />
=====void calibrate_color(void)=====<br />
This function runs the color calibration routine. Eventually this should be turned into a separate e-puck command from the vision system. Currently it just runs the puck through a zig zag pattern on the floor and sends packets. You can collect these with the data logger and process them in matlab. From this, you should be able to get a calibration function.<br />
<br />
'''To do:'''<br />
* Add to the pattern. Need to collect more data.<br />
* Possibly have the puck store data, find a best fit, and create the calibration function on board.<br />
* Store the calibration function in the EEPROM (flash memory) so it's non-volatile. The coefficients of function will be specific to each puck, so it would be nice to not have to program each puck, each time you change the battery.<br />
<br />
===dsPIC_XBeePackets(.c/.h)===<br />
* .c/.h: Contains functions and data structures for assembling and receiving XBee packets.<br />
<br />
=====int readPacket( void)=====<br />
Parses XBee data from UART2. Makes it accessable in the ''InPacket'' struct.<br />
<br />
The UART2 receive ISR takes error codes from this function and flashes the corresponding puck LED (absolute value of the error code).<br />
<br />
Error codes are:<br />
<br />
<code><pre><br />
//////////////ERROR CODES/////////////////<br />
// <br />
// -1: Timeout waiting for UART2<br />
// -2: Start delimiter wrong<br />
// -3: Checksum Error<br />
// -4: UART2 not ready at beginning<br />
// -5: API_ID unidentified<br />
// -6: Send Packet CCA failuue<br />
// -7: Modem Status packet failure<br />
//////////////////////////////////////////<br />
</pre></code><br />
<br />
===send_packet(.c/.h)===<br />
* .c: Contains the void send_packet(void) function which fills an array with data and calls the needed XBee functions to send a packet.<br />
* .h: Contains function prototype.<br />
<br />
=====void send_packet(void)=====<br />
* Creates ''packet'' array.<br />
* Adds consensus estimator data to the array.<br />
* Adds robot statistics to the array.<br />
* Adds color sensor values to the array.<br />
* Asserts flow control line to stop XBee from sending<br />
* Calls assemblePacket to send the packet.<br />
* Deasserts flow control; Xbee can send again.<br />
<br />
<br />
The function e_get_acc_filtered returns a running average of the acc specified. Syntax is:<br />
<br />
return value = e_get_acc_filtered(acc_channel, number of samples to average)<br />
<br />
The number of samples averaged must be less than ACC_SAMP_NB as defined in e_ad_conv.h.<br />
<br />
<code><pre><br />
packet[i]=e_get_acc_filtered(2, 136); // red<br />
i++;<br />
packet[i]=e_get_acc_filtered(1, 136); // green<br />
i++;<br />
packet[i]=e_get_acc_filtered(0, 136); // blue<br />
</pre></code><br />
<br />
===PI_consensus_estimator(.h)===<br />
* .h: Contains functions and data structures for the PI consensus estimator. <br />
* This will probably be replaced by the algorithm for sensor consensus. <br />
<br />
===wheel_speed_coordinator(.c/.h)===<br />
* .c: Contains functions for robot motion control<br />
* .h: Function prototypes and variabls.<br />
<br />
=====void wheelSpeed(int *vL, int *vR)=====<br />
Return needed wheel speeds for the inertial consensus estimator based on the group goal.<br />
<br />
=====void wheelSpeedSingleBot(float gotox, float gotoy, int *vL, int *vR)=====<br />
Return needed wheel speed to get this individual bot to (gotox, gotoy). It's a hacked fix. Should be replaced with the 3 step motion controller from NUtest.c.<br />
<br />
===e_acc(.c/.h)===<br />
* .c: Functions for reading the accelerometers (which is the color sensor).<br />
* .h: Function prototypes.<br />
<br />
This is original e-puck library code with the following modifications:<br />
<br />
<code><pre><br />
//changed by Sam, July 10, default offset is 2000. we want 0 for RGB sensor.<br />
static int centre_z = 0; //zero value for z axe<br />
</pre></code><br />
<br />
=====int e_get_acc_filtered(unsigned int captor, unsigned int filter_size)=====<br />
<br />
===e_ad_conv(.c/.h)===<br />
Set up the ADCs on the puck. Original e-puck library code, with the following modifications<br />
* .h: Define constants and functional prototypes<br />
<br />
MIC_SAMP_FREQ sets the baseline sampling frequency for the ADC, everything else must be a fraction of this. 16384 Hz is the highest possible.<br />
<code><pre><br />
#define MIC_SAMP_FREQ 16384.0 <br />
</pre></code><br />
<br />
ACC_PROX_SAMP_FREQ sets the sampling frequency for the accelerometers (color sensor). We found in testing that the puck become non-responsive with this set to 8192 Hz or 16384 Hz.<br />
<code><pre><br />
// sampling frequency for the accelerometres and proximetres<br />
//#define ACC_PROX_SAMP_FREQ 256.0 // WARNING: should be a fraction of MIC_SAMP_FREQ<br />
#define ACC_PROX_SAMP_FREQ 4096 // to ensure a good timing precision<br />
// So your options are: 1 2 4 8 16 32 64 128 <br />
// 256 512 1024 2048 4096 8192 16384<br />
</pre></code><br />
<br />
ACC_SAMP_NB is the number of samples to store. We can do an average of ''up to'' this many samples. This is set to 140 so we can average 136 samples, which is 4 projector periods.<br />
<code><pre><br />
#define ACC_SAMP_NB 140 // number of accelerometer samples to store<br />
</pre></code><br />
<br />
* .c: Functions and interrupt service routines for ADCs. Original e-puck library, no modifications.<br />
<br />
=====e_init_ad_scan(ALL_ADC)=====<br />
Call to setup ADC and have it work in the background. Use e_acc functions to access data.<br />
<br />
===e_init_port(.c/.h)===<br />
* .c/.h: Initializes the ports on the e-puck. File is from the standard e-puck library. <br />
<br />
=====e_init_ports(void)=====<br />
This function sets up ports on the e-puck. Call before using any ports.<br />
<br />
===e_led(.c/.h)===<br />
* .c/.h: This is a standard e-puck library file that contains functions for manipulating LEDs.<br />
<br />
=====void e_set_led(unsigned int led_number, unsigned int value)=====<br />
Set led_number (0-7) to value (0=off 1=on higher=inverse).<br />
<br />
[[Image:e-puck_LED_numbering.png|thumb|left]]<br />
<br />
<br clear='all'><br />
<br />
===e_motors_swarm(.c/.h)===<br />
* .c/.h: This file is a modified version of the e_motors.h e-puck library file. This version keeps track of the robot's position and orientation, and the motor stepping function contains code to update the robot's position when the wheels turn. <br />
* The functions like e_rotate and e_translate have been removed and this version is not dependent on e_agenda. <br />
<br />
=====#define POINT_OFFSET -31.75 =====<br />
In addition to constants like wheel radius and wheel base, the offset between the center point of the bot and the point we're driving is set in e_motors_swarm.h. -31.75 mm is the distance betwene the center and the color sensor. So technically, we're driving backwards.<br />
<br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T5Interrupt(void)=====<br />
Timer5 ISR, interrupt for left motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos + cval*LINSTEP;<br />
ypos = ypos + sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T4Interrupt(void)=====<br />
Timer4 ISR, interrupt for right motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos - cval*LINSTEP;<br />
ypos = ypos - sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void e_init_motors(void)=====<br />
Call this function before other motor functions to initialize the motors.<br />
<br />
=====void e_set_speed_left(int motor_speed)/void e_set_speed_right(int motor_speed)=====<br />
Set the motor speed in steps/second.<br />
<br />
=====void e_get_configuration(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the center reference point.<br />
<br />
=====void e_set_configuration(float x, float y, float theta)=====<br />
Sets x, y, theta to values. This is used to overwrite the puck position/orientation estimates with data from the vision system.<br />
<br />
=====void e_get_configuration_front(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the front reference point (used for motor control).<br />
<br />
<br />
[[Category:SwarmRobotProject]]<br />
[[Category:e-puck]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_E-puck_Code_(outdated)&diff=14276RGB Swarm Robot Project E-puck Code (outdated)2009-09-04T17:48:10Z<p>Sam Bobb: /* void e_set_configuration(float x, float y, float theta) */</p>
<hr />
<div>This page documents the e-puck code for the RGB Sensing Swarm Robotics project. The code on the e-puck was written in C and compiled using Microchip's MPLAB C Compiler for dsPIC DSCs (student version). <br />
<br />
This code is a branch of the [[Swarm Project E-puck Code]].<br />
<br />
==Tasks==<br />
<br />
===Complete===<br />
* Restructured code to make more modular.<br />
** Split dsPIC_XBeePackets and wheel_speed_coordinator into h and c files<br />
** Pulled packet assembling code out of main and created send_packet() function in send_packet.h/c.<br />
** Pulled a bunch of variables and defines (''NUM_DATA_SETS, NUMBERS_PER_SET, DATATYPE_BYTELENGTH , DATA_ARRAY_LENGTH , ADDITIONAL_NUMS, notRTS, T1_INT_FLAG, x_i, u_i, w_i, x_sum, w_sum, MAX_WHEEL_V_TICKS, deadband, COMMR, SAFEDIST, MINDIST, u_x_ideal, u_y_ideal, x_motion_integral, y_motion_integral, SQUARE'') that were scattered across h files into global_vars.h/c. Makes it easy to include them in a particular file with the ''extern'' keyword.<br />
* Added color_cal() function in color_cal.h/c<br />
* Added wheelSpeedSingleBot to wheel_speed_coordinator<br />
<br />
===To Do===<br />
* Finish color_cal<br />
* Make the vision system position information updater smarter.<br />
* Replace wheelSpeedSingleBot with the three step move controller from NUtest.c<br />
* Implement new algorithm<br />
<br />
<br />
==Project Package==<br />
The source code for the project is available here: [[Media:RGB_Swarm_Puck_Code_working_version.zip]]. Open swarm_epucks.mcw and you should be good to go.<br />
<br />
==Description of the files and functions==<br />
<br />
===global_vars(.c/.h)===<br />
* .c/.h: declare and define global variables and macros<br />
====Packet Length Constants====<br />
These variables determine the length of the XBee packets. See [[Swarm_Robot_Project_Documentation#Data_Frame|Data Frame]] and the section on XBee API packets in the XBee manual for further clarification.<br />
<br />
Much of this is still sending integral consensus estimator data. This can be removed or replaces with data needed for decentralized color sensing. <br />
<br />
=====#define NUM_DATA_SETS 5 ===== <br />
Number of statistics on which you are running the consensus estimator. This this particular case, 5. (Ix, Iy, Ixx, Ixy, Iyy)<br />
<br />
=====#define NUMBERS_PER_SET 2=====<br />
Number of variables in each data set (see above) that the consensus estimator needs to transmit to other agents. In this case, 2 because there is <tt>x_i</tt> and <tt>w_i</tt> for each statistic.<br />
<br />
=====#define DATATYPE_BYTELENGTH 4 =====<br />
Number of bytes in the data type (used in the consensus estimator (float = 4 bytes long). This is important because we need to split the numbers into individual bytes to be able to send them out the serial port.<br />
<br />
=====#define DATA_ARRAY_LENGTH (NUM_DATA_SETS*NUMBERS_PER_SET)=====<br />
Total number of data variables needed for the consensus estimator. In this case, it is 5*2=10.<br />
<br />
=====#define ADDITIONAL_NUMS 8=====<br />
Additional number of data to be appended to data array. It is 5 in this case, so that we can append <br />
# Robot X coordinate<br />
# Robot Y coordinate<br />
# Robot Theta orientation<br />
# Robot left wheel speed<br />
# Robot right wheel speed<br />
# Sensor Red Value<br />
# Sensor Green Value<br />
# Sensor Blue Value<br />
<br />
===main(.c/.h)===<br />
* .c: This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines. <br />
* .h: Contains variables, function prototypes, and delay function needed for main.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void)=====<br />
Timer1 ISR. Sets T1_INT_FLAG which provides timing for the main loop.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)=====<br />
UART2 receive ISR. Runs with the XBee receives packets. The switch/case structure handles the packets.<br />
<br />
The current handling of coordinate data from the vision system: <br />
<br />
<code><pre><br />
case 0: //coordinate data<br />
e_set_configuration(<br />
InPacket.data[1].dataFloat,<br />
InPacket.data[2].dataFloat, <br />
InPacket.data[3].dataFloat);<br />
break;<br />
</pre></code><br />
<br />
Needs to be improved. Suggested new function:<br />
<br />
* Get vision system data<br />
* Check if it's wildly off from current puck estimate<br />
** If not, update, clear log of rejected packets<br />
** If so, reject and log<br />
*** If we have rejected enough packets (some threshold) and they've all been in similar place (within tolerance), we can assume the puck is wrong and vision system is right. In this case, update with vision system data and clear rejected data log.<br />
<br />
=====int main(void)=====<br />
Setup functions and main loop.<br />
<br />
===color_cal(.c/.h)===<br />
* .c: Contains void calibrate_color(void) function to run the calibration routine.<br />
* .h: Contains function prototype and constant definitions for calibrate_color.<br />
<br />
=====void calibrate_color(void)=====<br />
This function runs the color calibration routine. Eventually this should be turned into a separate e-puck command from the vision system. Currently it just runs the puck through a zig zag pattern on the floor and sends packets. You can collect these with the data logger and process them in matlab. From this, you should be able to get a calibration function.<br />
<br />
'''To do:'''<br />
* Add to the pattern. Need to collect more data.<br />
* Possibly have the puck store data, find a best fit, and create the calibration function on board.<br />
* Store the calibration function in the EEPROM (flash memory) so it's non-volatile. The coefficients of function will be specific to each puck, so it would be nice to not have to program each puck, each time you change the battery.<br />
<br />
===dsPIC_XBeePackets(.c/.h)===<br />
* .c/.h: Contains functions and data structures for assembling and receiving XBee packets.<br />
<br />
=====int readPacket( void)=====<br />
Parses XBee data from UART2. Makes it accessable in the ''InPacket'' struct.<br />
<br />
The UART2 receive ISR takes error codes from this function and flashes the corresponding puck LED (absolute value of the error code).<br />
<br />
Error codes are:<br />
<br />
<code><pre><br />
//////////////ERROR CODES/////////////////<br />
// <br />
// -1: Timeout waiting for UART2<br />
// -2: Start delimiter wrong<br />
// -3: Checksum Error<br />
// -4: UART2 not ready at beginning<br />
// -5: API_ID unidentified<br />
// -6: Send Packet CCA failuue<br />
// -7: Modem Status packet failure<br />
//////////////////////////////////////////<br />
</pre></code><br />
<br />
===send_packet(.c/.h)===<br />
* .c: Contains the void send_packet(void) function which fills an array with data and calls the needed XBee functions to send a packet.<br />
* .h: Contains function prototype.<br />
<br />
=====void send_packet(void)=====<br />
* Creates ''packet'' array.<br />
* Adds consensus estimator data to the array.<br />
* Adds robot statistics to the array.<br />
* Adds color sensor values to the array.<br />
* Asserts flow control line to stop XBee from sending<br />
* Calls assemblePacket to send the packet.<br />
* Deasserts flow control; Xbee can send again.<br />
<br />
<br />
The function e_get_acc_filtered returns a running average of the acc specified. Syntax is:<br />
<br />
return value = e_get_acc_filtered(acc_channel, number of samples to average)<br />
<br />
The number of samples averaged must be less than ACC_SAMP_NB as defined in e_ad_conv.h.<br />
<br />
<code><pre><br />
packet[i]=e_get_acc_filtered(2, 136); // red<br />
i++;<br />
packet[i]=e_get_acc_filtered(1, 136); // green<br />
i++;<br />
packet[i]=e_get_acc_filtered(0, 136); // blue<br />
</pre></code><br />
<br />
===PI_consensus_estimator(.h)===<br />
* .h: Contains functions and data structures for the PI consensus estimator. <br />
* This will probably be replaced by the algorithm for sensor consensus. <br />
<br />
===wheel_speed_coordinator(.c/.h)===<br />
* .c: Contains functions for robot motion control<br />
* .h: Function prototypes and variabls.<br />
<br />
=====void wheelSpeed(int *vL, int *vR)=====<br />
Return needed wheel speeds for the inertial consensus estimator based on the group goal.<br />
<br />
=====void wheelSpeedSingleBot(float gotox, float gotoy, int *vL, int *vR)=====<br />
Return needed wheel speed to get this individual bot to (gotox, gotoy). It's a hacked fix. Should be replaced with the 3 step motion controller from NUtest.c.<br />
<br />
===e_acc(.c/.h)===<br />
* .c: Functions for reading the accelerometers (which is the color sensor).<br />
* .h: Function prototypes.<br />
<br />
This is original e-puck library code with the following modifications:<br />
<br />
<code><pre><br />
//changed by Sam, July 10, default offset is 2000. we want 0 for RGB sensor.<br />
static int centre_z = 0; //zero value for z axe<br />
</pre></code><br />
<br />
=====int e_get_acc_filtered(unsigned int captor, unsigned int filter_size)=====<br />
<br />
===e_ad_conv(.c/.h)===<br />
Set up the ADCs on the puck. Original e-puck library code, with the following modifications<br />
* .h: Define constants and functional prototypes<br />
<br />
MIC_SAMP_FREQ sets the baseline sampling frequency for the ADC, everything else must be a fraction of this. 16384 Hz is the highest possible.<br />
<code><pre><br />
#define MIC_SAMP_FREQ 16384.0 <br />
</pre></code><br />
<br />
ACC_PROX_SAMP_FREQ sets the sampling frequency for the accelerometers (color sensor). We found in testing that the puck become non-responsive with this set to 8192 Hz or 16384 Hz.<br />
<code><pre><br />
// sampling frequency for the accelerometres and proximetres<br />
//#define ACC_PROX_SAMP_FREQ 256.0 // WARNING: should be a fraction of MIC_SAMP_FREQ<br />
#define ACC_PROX_SAMP_FREQ 4096 // to ensure a good timing precision<br />
// So your options are: 1 2 4 8 16 32 64 128 <br />
// 256 512 1024 2048 4096 8192 16384<br />
</pre></code><br />
<br />
ACC_SAMP_NB is the number of samples to store. We can do an average of ''up to'' this many samples. This is set to 140 so we can average 136 samples, which is 4 projector periods.<br />
<code><pre><br />
#define ACC_SAMP_NB 140 // number of accelerometer samples to store<br />
</pre></code><br />
<br />
* .c: Functions and interrupt service routines for ADCs. Original e-puck library, no modifications.<br />
<br />
=====e_init_ad_scan(ALL_ADC)=====<br />
Call to setup ADC and have it work in the background. Use e_acc functions to access data.<br />
<br />
===e_init_port(.c/.h)===<br />
* .c/.h: Initializes the ports on the e-puck. File is from the standard e-puck library. <br />
<br />
=====e_init_ports(void)=====<br />
This function sets up ports on the e-puck. Call before using any ports.<br />
<br />
===e_led(.c/.h)===<br />
* .c/.h: This is a standard e-puck library file that contains functions for manipulating LEDs.<br />
<br />
=====void e_set_led(unsigned int led_number, unsigned int value)=====<br />
Set led_number (0-7) to value (0=off 1=on higher=inverse).<br />
<br />
[[Image:e-puck_LED_numbering.png|thumb|left]]<br />
<br />
<br clear='all'><br />
<br />
===e_motors_swarm(.c/.h)===<br />
* .c/.h: This file is a modified version of the e_motors.h e-puck library file. This version keeps track of the robot's position and orientation, and the motor stepping function contains code to update the robot's position when the wheels turn. <br />
* The functions like e_rotate and e_translate have been removed and this version is not dependent on e_agenda. <br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T5Interrupt(void)=====<br />
Timer5 ISR, interrupt for left motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos + cval*LINSTEP;<br />
ypos = ypos + sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T4Interrupt(void)=====<br />
Timer4 ISR, interrupt for right motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos - cval*LINSTEP;<br />
ypos = ypos - sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void e_init_motors(void)=====<br />
Call this function before other motor functions to initialize the motors.<br />
<br />
=====void e_set_speed_left(int motor_speed)/void e_set_speed_right(int motor_speed)=====<br />
Set the motor speed in steps/second.<br />
<br />
=====void e_get_configuration(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the center reference point.<br />
<br />
=====void e_set_configuration(float x, float y, float theta)=====<br />
Sets x, y, theta to values. This is used to overwrite the puck position/orientation estimates with data from the vision system.<br />
<br />
=====void e_get_configuration_front(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the front reference point (used for motor control).<br />
<br />
<br />
[[Category:SwarmRobotProject]]<br />
[[Category:e-puck]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_E-puck_Code_(outdated)&diff=14274RGB Swarm Robot Project E-puck Code (outdated)2009-09-04T17:46:54Z<p>Sam Bobb: /* e_init_ports(void) */</p>
<hr />
<div>This page documents the e-puck code for the RGB Sensing Swarm Robotics project. The code on the e-puck was written in C and compiled using Microchip's MPLAB C Compiler for dsPIC DSCs (student version). <br />
<br />
This code is a branch of the [[Swarm Project E-puck Code]].<br />
<br />
==Tasks==<br />
<br />
===Complete===<br />
* Restructured code to make more modular.<br />
** Split dsPIC_XBeePackets and wheel_speed_coordinator into h and c files<br />
** Pulled packet assembling code out of main and created send_packet() function in send_packet.h/c.<br />
** Pulled a bunch of variables and defines (''NUM_DATA_SETS, NUMBERS_PER_SET, DATATYPE_BYTELENGTH , DATA_ARRAY_LENGTH , ADDITIONAL_NUMS, notRTS, T1_INT_FLAG, x_i, u_i, w_i, x_sum, w_sum, MAX_WHEEL_V_TICKS, deadband, COMMR, SAFEDIST, MINDIST, u_x_ideal, u_y_ideal, x_motion_integral, y_motion_integral, SQUARE'') that were scattered across h files into global_vars.h/c. Makes it easy to include them in a particular file with the ''extern'' keyword.<br />
* Added color_cal() function in color_cal.h/c<br />
* Added wheelSpeedSingleBot to wheel_speed_coordinator<br />
<br />
===To Do===<br />
* Finish color_cal<br />
* Make the vision system position information updater smarter.<br />
* Replace wheelSpeedSingleBot with the three step move controller from NUtest.c<br />
* Implement new algorithm<br />
<br />
<br />
==Project Package==<br />
The source code for the project is available here: [[Media:RGB_Swarm_Puck_Code_working_version.zip]]. Open swarm_epucks.mcw and you should be good to go.<br />
<br />
==Description of the files and functions==<br />
<br />
===global_vars(.c/.h)===<br />
* .c/.h: declare and define global variables and macros<br />
====Packet Length Constants====<br />
These variables determine the length of the XBee packets. See [[Swarm_Robot_Project_Documentation#Data_Frame|Data Frame]] and the section on XBee API packets in the XBee manual for further clarification.<br />
<br />
Much of this is still sending integral consensus estimator data. This can be removed or replaces with data needed for decentralized color sensing. <br />
<br />
=====#define NUM_DATA_SETS 5 ===== <br />
Number of statistics on which you are running the consensus estimator. This this particular case, 5. (Ix, Iy, Ixx, Ixy, Iyy)<br />
<br />
=====#define NUMBERS_PER_SET 2=====<br />
Number of variables in each data set (see above) that the consensus estimator needs to transmit to other agents. In this case, 2 because there is <tt>x_i</tt> and <tt>w_i</tt> for each statistic.<br />
<br />
=====#define DATATYPE_BYTELENGTH 4 =====<br />
Number of bytes in the data type (used in the consensus estimator (float = 4 bytes long). This is important because we need to split the numbers into individual bytes to be able to send them out the serial port.<br />
<br />
=====#define DATA_ARRAY_LENGTH (NUM_DATA_SETS*NUMBERS_PER_SET)=====<br />
Total number of data variables needed for the consensus estimator. In this case, it is 5*2=10.<br />
<br />
=====#define ADDITIONAL_NUMS 8=====<br />
Additional number of data to be appended to data array. It is 5 in this case, so that we can append <br />
# Robot X coordinate<br />
# Robot Y coordinate<br />
# Robot Theta orientation<br />
# Robot left wheel speed<br />
# Robot right wheel speed<br />
# Sensor Red Value<br />
# Sensor Green Value<br />
# Sensor Blue Value<br />
<br />
===main(.c/.h)===<br />
* .c: This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines. <br />
* .h: Contains variables, function prototypes, and delay function needed for main.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void)=====<br />
Timer1 ISR. Sets T1_INT_FLAG which provides timing for the main loop.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)=====<br />
UART2 receive ISR. Runs with the XBee receives packets. The switch/case structure handles the packets.<br />
<br />
The current handling of coordinate data from the vision system: <br />
<br />
<code><pre><br />
case 0: //coordinate data<br />
e_set_configuration(<br />
InPacket.data[1].dataFloat,<br />
InPacket.data[2].dataFloat, <br />
InPacket.data[3].dataFloat);<br />
break;<br />
</pre></code><br />
<br />
Needs to be improved. Suggested new function:<br />
<br />
* Get vision system data<br />
* Check if it's wildly off from current puck estimate<br />
** If not, update, clear log of rejected packets<br />
** If so, reject and log<br />
*** If we have rejected enough packets (some threshold) and they've all been in similar place (within tolerance), we can assume the puck is wrong and vision system is right. In this case, update with vision system data and clear rejected data log.<br />
<br />
=====int main(void)=====<br />
Setup functions and main loop.<br />
<br />
===color_cal(.c/.h)===<br />
* .c: Contains void calibrate_color(void) function to run the calibration routine.<br />
* .h: Contains function prototype and constant definitions for calibrate_color.<br />
<br />
=====void calibrate_color(void)=====<br />
This function runs the color calibration routine. Eventually this should be turned into a separate e-puck command from the vision system. Currently it just runs the puck through a zig zag pattern on the floor and sends packets. You can collect these with the data logger and process them in matlab. From this, you should be able to get a calibration function.<br />
<br />
'''To do:'''<br />
* Add to the pattern. Need to collect more data.<br />
* Possibly have the puck store data, find a best fit, and create the calibration function on board.<br />
* Store the calibration function in the EEPROM (flash memory) so it's non-volatile. The coefficients of function will be specific to each puck, so it would be nice to not have to program each puck, each time you change the battery.<br />
<br />
===dsPIC_XBeePackets(.c/.h)===<br />
* .c/.h: Contains functions and data structures for assembling and receiving XBee packets.<br />
<br />
=====int readPacket( void)=====<br />
Parses XBee data from UART2. Makes it accessable in the ''InPacket'' struct.<br />
<br />
The UART2 receive ISR takes error codes from this function and flashes the corresponding puck LED (absolute value of the error code).<br />
<br />
Error codes are:<br />
<br />
<code><pre><br />
//////////////ERROR CODES/////////////////<br />
// <br />
// -1: Timeout waiting for UART2<br />
// -2: Start delimiter wrong<br />
// -3: Checksum Error<br />
// -4: UART2 not ready at beginning<br />
// -5: API_ID unidentified<br />
// -6: Send Packet CCA failuue<br />
// -7: Modem Status packet failure<br />
//////////////////////////////////////////<br />
</pre></code><br />
<br />
===send_packet(.c/.h)===<br />
* .c: Contains the void send_packet(void) function which fills an array with data and calls the needed XBee functions to send a packet.<br />
* .h: Contains function prototype.<br />
<br />
=====void send_packet(void)=====<br />
* Creates ''packet'' array.<br />
* Adds consensus estimator data to the array.<br />
* Adds robot statistics to the array.<br />
* Adds color sensor values to the array.<br />
* Asserts flow control line to stop XBee from sending<br />
* Calls assemblePacket to send the packet.<br />
* Deasserts flow control; Xbee can send again.<br />
<br />
<br />
The function e_get_acc_filtered returns a running average of the acc specified. Syntax is:<br />
<br />
return value = e_get_acc_filtered(acc_channel, number of samples to average)<br />
<br />
The number of samples averaged must be less than ACC_SAMP_NB as defined in e_ad_conv.h.<br />
<br />
<code><pre><br />
packet[i]=e_get_acc_filtered(2, 136); // red<br />
i++;<br />
packet[i]=e_get_acc_filtered(1, 136); // green<br />
i++;<br />
packet[i]=e_get_acc_filtered(0, 136); // blue<br />
</pre></code><br />
<br />
===PI_consensus_estimator(.h)===<br />
* .h: Contains functions and data structures for the PI consensus estimator. <br />
* This will probably be replaced by the algorithm for sensor consensus. <br />
<br />
===wheel_speed_coordinator(.c/.h)===<br />
* .c: Contains functions for robot motion control<br />
* .h: Function prototypes and variabls.<br />
<br />
=====void wheelSpeed(int *vL, int *vR)=====<br />
Return needed wheel speeds for the inertial consensus estimator based on the group goal.<br />
<br />
=====void wheelSpeedSingleBot(float gotox, float gotoy, int *vL, int *vR)=====<br />
Return needed wheel speed to get this individual bot to (gotox, gotoy). It's a hacked fix. Should be replaced with the 3 step motion controller from NUtest.c.<br />
<br />
===e_acc(.c/.h)===<br />
* .c: Functions for reading the accelerometers (which is the color sensor).<br />
* .h: Function prototypes.<br />
<br />
This is original e-puck library code with the following modifications:<br />
<br />
<code><pre><br />
//changed by Sam, July 10, default offset is 2000. we want 0 for RGB sensor.<br />
static int centre_z = 0; //zero value for z axe<br />
</pre></code><br />
<br />
=====int e_get_acc_filtered(unsigned int captor, unsigned int filter_size)=====<br />
<br />
===e_ad_conv(.c/.h)===<br />
Set up the ADCs on the puck. Original e-puck library code, with the following modifications<br />
* .h: Define constants and functional prototypes<br />
<br />
MIC_SAMP_FREQ sets the baseline sampling frequency for the ADC, everything else must be a fraction of this. 16384 Hz is the highest possible.<br />
<code><pre><br />
#define MIC_SAMP_FREQ 16384.0 <br />
</pre></code><br />
<br />
ACC_PROX_SAMP_FREQ sets the sampling frequency for the accelerometers (color sensor). We found in testing that the puck become non-responsive with this set to 8192 Hz or 16384 Hz.<br />
<code><pre><br />
// sampling frequency for the accelerometres and proximetres<br />
//#define ACC_PROX_SAMP_FREQ 256.0 // WARNING: should be a fraction of MIC_SAMP_FREQ<br />
#define ACC_PROX_SAMP_FREQ 4096 // to ensure a good timing precision<br />
// So your options are: 1 2 4 8 16 32 64 128 <br />
// 256 512 1024 2048 4096 8192 16384<br />
</pre></code><br />
<br />
ACC_SAMP_NB is the number of samples to store. We can do an average of ''up to'' this many samples. This is set to 140 so we can average 136 samples, which is 4 projector periods.<br />
<code><pre><br />
#define ACC_SAMP_NB 140 // number of accelerometer samples to store<br />
</pre></code><br />
<br />
* .c: Functions and interrupt service routines for ADCs. Original e-puck library, no modifications.<br />
<br />
=====e_init_ad_scan(ALL_ADC)=====<br />
Call to setup ADC and have it work in the background. Use e_acc functions to access data.<br />
<br />
===e_init_port(.c/.h)===<br />
* .c/.h: Initializes the ports on the e-puck. File is from the standard e-puck library. <br />
<br />
=====e_init_ports(void)=====<br />
This function sets up ports on the e-puck. Call before using any ports.<br />
<br />
===e_led(.c/.h)===<br />
* .c/.h: This is a standard e-puck library file that contains functions for manipulating LEDs.<br />
<br />
=====void e_set_led(unsigned int led_number, unsigned int value)=====<br />
Set led_number (0-7) to value (0=off 1=on higher=inverse).<br />
<br />
[[Image:e-puck_LED_numbering.png|thumb|left]]<br />
<br />
<br clear='all'><br />
<br />
===e_motors_swarm(.c/.h)===<br />
* .c/.h: This file is a modified version of the e_motors.h e-puck library file. This version keeps track of the robot's position and orientation, and the motor stepping function contains code to update the robot's position when the wheels turn. <br />
* The functions like e_rotate and e_translate have been removed and this version is not dependent on e_agenda. <br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T5Interrupt(void)=====<br />
Timer5 ISR, interrupt for left motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos + cval*LINSTEP;<br />
ypos = ypos + sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T4Interrupt(void)=====<br />
Timer4 ISR, interrupt for right motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos - cval*LINSTEP;<br />
ypos = ypos - sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void e_init_motors(void)=====<br />
Call this function before other motor functions to initialize the motors.<br />
<br />
=====void e_set_speed_left(int motor_speed)/void e_set_speed_right(int motor_speed)=====<br />
Set the motor speed in steps/second.<br />
<br />
=====void e_get_configuration(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the center reference point.<br />
<br />
=====void e_set_configuration(float x, float y, float theta)=====<br />
Sets x, y, theta to values.<br />
<br />
=====void e_get_configuration_front(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the front reference point (used for motor control).<br />
<br />
<br />
[[Category:SwarmRobotProject]]<br />
[[Category:e-puck]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_E-puck_Code_(outdated)&diff=14272RGB Swarm Robot Project E-puck Code (outdated)2009-09-04T17:43:57Z<p>Sam Bobb: /* Description of the files and functions */</p>
<hr />
<div>This page documents the e-puck code for the RGB Sensing Swarm Robotics project. The code on the e-puck was written in C and compiled using Microchip's MPLAB C Compiler for dsPIC DSCs (student version). <br />
<br />
This code is a branch of the [[Swarm Project E-puck Code]].<br />
<br />
==Tasks==<br />
<br />
===Complete===<br />
* Restructured code to make more modular.<br />
** Split dsPIC_XBeePackets and wheel_speed_coordinator into h and c files<br />
** Pulled packet assembling code out of main and created send_packet() function in send_packet.h/c.<br />
** Pulled a bunch of variables and defines (''NUM_DATA_SETS, NUMBERS_PER_SET, DATATYPE_BYTELENGTH , DATA_ARRAY_LENGTH , ADDITIONAL_NUMS, notRTS, T1_INT_FLAG, x_i, u_i, w_i, x_sum, w_sum, MAX_WHEEL_V_TICKS, deadband, COMMR, SAFEDIST, MINDIST, u_x_ideal, u_y_ideal, x_motion_integral, y_motion_integral, SQUARE'') that were scattered across h files into global_vars.h/c. Makes it easy to include them in a particular file with the ''extern'' keyword.<br />
* Added color_cal() function in color_cal.h/c<br />
* Added wheelSpeedSingleBot to wheel_speed_coordinator<br />
<br />
===To Do===<br />
* Finish color_cal<br />
* Make the vision system position information updater smarter.<br />
* Replace wheelSpeedSingleBot with the three step move controller from NUtest.c<br />
* Implement new algorithm<br />
<br />
<br />
==Project Package==<br />
The source code for the project is available here: [[Media:RGB_Swarm_Puck_Code_working_version.zip]]. Open swarm_epucks.mcw and you should be good to go.<br />
<br />
==Description of the files and functions==<br />
<br />
===global_vars(.c/.h)===<br />
* .c/.h: declare and define global variables and macros<br />
====Packet Length Constants====<br />
These variables determine the length of the XBee packets. See [[Swarm_Robot_Project_Documentation#Data_Frame|Data Frame]] and the section on XBee API packets in the XBee manual for further clarification.<br />
<br />
Much of this is still sending integral consensus estimator data. This can be removed or replaces with data needed for decentralized color sensing. <br />
<br />
=====#define NUM_DATA_SETS 5 ===== <br />
Number of statistics on which you are running the consensus estimator. This this particular case, 5. (Ix, Iy, Ixx, Ixy, Iyy)<br />
<br />
=====#define NUMBERS_PER_SET 2=====<br />
Number of variables in each data set (see above) that the consensus estimator needs to transmit to other agents. In this case, 2 because there is <tt>x_i</tt> and <tt>w_i</tt> for each statistic.<br />
<br />
=====#define DATATYPE_BYTELENGTH 4 =====<br />
Number of bytes in the data type (used in the consensus estimator (float = 4 bytes long). This is important because we need to split the numbers into individual bytes to be able to send them out the serial port.<br />
<br />
=====#define DATA_ARRAY_LENGTH (NUM_DATA_SETS*NUMBERS_PER_SET)=====<br />
Total number of data variables needed for the consensus estimator. In this case, it is 5*2=10.<br />
<br />
=====#define ADDITIONAL_NUMS 8=====<br />
Additional number of data to be appended to data array. It is 5 in this case, so that we can append <br />
# Robot X coordinate<br />
# Robot Y coordinate<br />
# Robot Theta orientation<br />
# Robot left wheel speed<br />
# Robot right wheel speed<br />
# Sensor Red Value<br />
# Sensor Green Value<br />
# Sensor Blue Value<br />
<br />
===main(.c/.h)===<br />
* .c: This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines. <br />
* .h: Contains variables, function prototypes, and delay function needed for main.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void)=====<br />
Timer1 ISR. Sets T1_INT_FLAG which provides timing for the main loop.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)=====<br />
UART2 receive ISR. Runs with the XBee receives packets. The switch/case structure handles the packets.<br />
<br />
The current handling of coordinate data from the vision system: <br />
<br />
<code><pre><br />
case 0: //coordinate data<br />
e_set_configuration(<br />
InPacket.data[1].dataFloat,<br />
InPacket.data[2].dataFloat, <br />
InPacket.data[3].dataFloat);<br />
break;<br />
</pre></code><br />
<br />
Needs to be improved. Suggested new function:<br />
<br />
* Get vision system data<br />
* Check if it's wildly off from current puck estimate<br />
** If not, update, clear log of rejected packets<br />
** If so, reject and log<br />
*** If we have rejected enough packets (some threshold) and they've all been in similar place (within tolerance), we can assume the puck is wrong and vision system is right. In this case, update with vision system data and clear rejected data log.<br />
<br />
=====int main(void)=====<br />
Setup functions and main loop.<br />
<br />
===color_cal(.c/.h)===<br />
* .c: Contains void calibrate_color(void) function to run the calibration routine.<br />
* .h: Contains function prototype and constant definitions for calibrate_color.<br />
<br />
=====void calibrate_color(void)=====<br />
This function runs the color calibration routine. Eventually this should be turned into a separate e-puck command from the vision system. Currently it just runs the puck through a zig zag pattern on the floor and sends packets. You can collect these with the data logger and process them in matlab. From this, you should be able to get a calibration function.<br />
<br />
'''To do:'''<br />
* Add to the pattern. Need to collect more data.<br />
* Possibly have the puck store data, find a best fit, and create the calibration function on board.<br />
* Store the calibration function in the EEPROM (flash memory) so it's non-volatile. The coefficients of function will be specific to each puck, so it would be nice to not have to program each puck, each time you change the battery.<br />
<br />
===dsPIC_XBeePackets(.c/.h)===<br />
* .c/.h: Contains functions and data structures for assembling and receiving XBee packets.<br />
<br />
=====int readPacket( void)=====<br />
Parses XBee data from UART2. Makes it accessable in the ''InPacket'' struct.<br />
<br />
The UART2 receive ISR takes error codes from this function and flashes the corresponding puck LED (absolute value of the error code).<br />
<br />
Error codes are:<br />
<br />
<code><pre><br />
//////////////ERROR CODES/////////////////<br />
// <br />
// -1: Timeout waiting for UART2<br />
// -2: Start delimiter wrong<br />
// -3: Checksum Error<br />
// -4: UART2 not ready at beginning<br />
// -5: API_ID unidentified<br />
// -6: Send Packet CCA failuue<br />
// -7: Modem Status packet failure<br />
//////////////////////////////////////////<br />
</pre></code><br />
<br />
===send_packet(.c/.h)===<br />
* .c: Contains the void send_packet(void) function which fills an array with data and calls the needed XBee functions to send a packet.<br />
* .h: Contains function prototype.<br />
<br />
=====void send_packet(void)=====<br />
* Creates ''packet'' array.<br />
* Adds consensus estimator data to the array.<br />
* Adds robot statistics to the array.<br />
* Adds color sensor values to the array.<br />
* Asserts flow control line to stop XBee from sending<br />
* Calls assemblePacket to send the packet.<br />
* Deasserts flow control; Xbee can send again.<br />
<br />
<br />
The function e_get_acc_filtered returns a running average of the acc specified. Syntax is:<br />
<br />
return value = e_get_acc_filtered(acc_channel, number of samples to average)<br />
<br />
The number of samples averaged must be less than ACC_SAMP_NB as defined in e_ad_conv.h.<br />
<br />
<code><pre><br />
packet[i]=e_get_acc_filtered(2, 136); // red<br />
i++;<br />
packet[i]=e_get_acc_filtered(1, 136); // green<br />
i++;<br />
packet[i]=e_get_acc_filtered(0, 136); // blue<br />
</pre></code><br />
<br />
===PI_consensus_estimator(.h)===<br />
* .h: Contains functions and data structures for the PI consensus estimator. <br />
* This will probably be replaced by the algorithm for sensor consensus. <br />
<br />
===wheel_speed_coordinator(.c/.h)===<br />
* .c: Contains functions for robot motion control<br />
* .h: Function prototypes and variabls.<br />
<br />
=====void wheelSpeed(int *vL, int *vR)=====<br />
Return needed wheel speeds for the inertial consensus estimator based on the group goal.<br />
<br />
=====void wheelSpeedSingleBot(float gotox, float gotoy, int *vL, int *vR)=====<br />
Return needed wheel speed to get this individual bot to (gotox, gotoy). It's a hacked fix. Should be replaced with the 3 step motion controller from NUtest.c.<br />
<br />
===e_acc(.c/.h)===<br />
* .c: Functions for reading the accelerometers (which is the color sensor).<br />
* .h: Function prototypes.<br />
<br />
This is original e-puck library code with the following modifications:<br />
<br />
<code><pre><br />
//changed by Sam, July 10, default offset is 2000. we want 0 for RGB sensor.<br />
static int centre_z = 0; //zero value for z axe<br />
</pre></code><br />
<br />
=====int e_get_acc_filtered(unsigned int captor, unsigned int filter_size)=====<br />
<br />
===e_ad_conv(.c/.h)===<br />
Set up the ADCs on the puck. Original e-puck library code, with the following modifications<br />
* .h: Define constants and functional prototypes<br />
<br />
MIC_SAMP_FREQ sets the baseline sampling frequency for the ADC, everything else must be a fraction of this. 16384 Hz is the highest possible.<br />
<code><pre><br />
#define MIC_SAMP_FREQ 16384.0 <br />
</pre></code><br />
<br />
ACC_PROX_SAMP_FREQ sets the sampling frequency for the accelerometers (color sensor). We found in testing that the puck become non-responsive with this set to 8192 Hz or 16384 Hz.<br />
<code><pre><br />
// sampling frequency for the accelerometres and proximetres<br />
//#define ACC_PROX_SAMP_FREQ 256.0 // WARNING: should be a fraction of MIC_SAMP_FREQ<br />
#define ACC_PROX_SAMP_FREQ 4096 // to ensure a good timing precision<br />
// So your options are: 1 2 4 8 16 32 64 128 <br />
// 256 512 1024 2048 4096 8192 16384<br />
</pre></code><br />
<br />
ACC_SAMP_NB is the number of samples to store. We can do an average of ''up to'' this many samples. This is set to 140 so we can average 136 samples, which is 4 projector periods.<br />
<code><pre><br />
#define ACC_SAMP_NB 140 // number of accelerometer samples to store<br />
</pre></code><br />
<br />
* .c: Functions and interrupt service routines for ADCs. Original e-puck library, no modifications.<br />
<br />
=====e_init_ad_scan(ALL_ADC)=====<br />
Call to setup ADC and have it work in the background. Use e_acc functions to access data.<br />
<br />
===e_init_port(.c/.h)===<br />
* .c/.h: Initializes the ports on the e-puck. File is from the standard e-puck library. <br />
<br />
=====e_init_ports(void)=====<br />
This function sets up ports on the e-puck.<br />
<br />
===e_led(.c/.h)===<br />
* .c/.h: This is a standard e-puck library file that contains functions for manipulating LEDs.<br />
<br />
=====void e_set_led(unsigned int led_number, unsigned int value)=====<br />
Set led_number (0-7) to value (0=off 1=on higher=inverse).<br />
<br />
[[Image:e-puck_LED_numbering.png|thumb|left]]<br />
<br />
<br clear='all'><br />
<br />
===e_motors_swarm(.c/.h)===<br />
* .c/.h: This file is a modified version of the e_motors.h e-puck library file. This version keeps track of the robot's position and orientation, and the motor stepping function contains code to update the robot's position when the wheels turn. <br />
* The functions like e_rotate and e_translate have been removed and this version is not dependent on e_agenda. <br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T5Interrupt(void)=====<br />
Timer5 ISR, interrupt for left motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos + cval*LINSTEP;<br />
ypos = ypos + sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T4Interrupt(void)=====<br />
Timer4 ISR, interrupt for right motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos - cval*LINSTEP;<br />
ypos = ypos - sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void e_init_motors(void)=====<br />
Call this function before other motor functions to initialize the motors.<br />
<br />
=====void e_set_speed_left(int motor_speed)/void e_set_speed_right(int motor_speed)=====<br />
Set the motor speed in steps/second.<br />
<br />
=====void e_get_configuration(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the center reference point.<br />
<br />
=====void e_set_configuration(float x, float y, float theta)=====<br />
Sets x, y, theta to values.<br />
<br />
=====void e_get_configuration_front(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the front reference point (used for motor control).<br />
<br />
<br />
[[Category:SwarmRobotProject]]<br />
[[Category:e-puck]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=File:RGB_Swarm_Puck_Code_working_version.zip&diff=14270File:RGB Swarm Puck Code working version.zip2009-09-04T17:40:50Z<p>Sam Bobb: uploaded a new version of "Image:RGB Swarm Puck Code working version.zip"</p>
<hr />
<div></div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_Documentation&diff=14269RGB Swarm Robot Project Documentation2009-09-04T17:39:00Z<p>Sam Bobb: /* RGB Swarm E-puck Code Overview */</p>
<hr />
<div>== Overview ==<br />
The swarm robot project has gone through several phases, with each phase focusing on different aspects of swarm robotics and the implementation of the project. This entry focuses on the most recent phase of the project, covering topics such as, but not limited to, '''Xbee Interface Extension Boards''', '''LED light boards''', and '''changes made to the Machine Vision Localization System''', and the overall conversion to LED boards and a controlled light environment. These entries help provide insight into setup and specific details to allow others to replicate or reproduce our results, and to provide additional information for those working on similar projects or this project at a later time. Other articles in the '''Swarm Robot Project''' category focus on topics such as the swarm theory and algorithms implemented, as well as previous phases of the project, such as motion control and consensus estimation. You may reach these articles and others by following the category link at the bottom of every page, or through this link - [[:Category:SwarmRobotProject|'''Swarm Robot Project''']].<br />
<br />
== Hardware ==<br />
<br />
<br />
<br />
===XBee Interface Extension Board Version 2===<br />
<br />
{|<br />
| [[Image:XBee_interface_extenstion_board_v1.gif|250px|thumb|alt=Traxmaker Image of the Previous Xbee Extension Board|Xbee Interface Extension Board Version]]<br />
| [[Image:IMG 1390-1-.jpg|300px|thumb|alt=Image of an e-Puck with the RGB Xbee Extension Board|e-Puck with previous board ]]<br />
| [[Image:XBee_interface_extenstion_board_v2.gif|vertical|250px|thumb|alt=Traxmaker Image of the Xbee Interface Exension Board Version 2|Xbee Interface Extension Board Version 2]]<br />
| <br />
|}<br />
<br />
====Previous Version====<br />
<br />
The previous version of XBee Interface Extension Board, designed by Michael Hwang.<br />
Its configuration is shown in the figure on the left, with an actual image of the board mounted on an e-Puck seen in the figure in the center. This version of the XBee Interface Board does not contain a color sensor in it. Details about this version of XBee Interface Extension Board, such as parts used and Traxmaker files can be found on the [[Swarm_Robot_Project_Documentation#Current_Version|Swarm Robot Project Documentation page]].<br />
<br clear="all"><br />
<br />
====Version 2====<br />
Th is the updated version of the Xbee board, or XBee Interface Extension Board Version 2. It is designed by Michael Hwang to accommodate further projects in the Swarm Robot Project. For this reason, the Xbee Interface Extension Board Version 2 has a color sensor circuit built in. The details of the color sensor circuit can be found in the color sensor section below. A copy of the Traxmaker PCB file for the Xbee Board Version 2 can be found at [[Media:epuck_xbee_board_v2.zip|'''Xbee Interface Extension Board Version 2''']].<br />
<br />
The RTS flow control line on the XBee is connected to the sel3 line of the e-puck. Although the CTS line is not connected to the sel2 pin in this board design, it can be easily connected with a jumper. <br />
<br />
The XBee Interface Extension Board Version 2 design was actually built and implemented on the e-puck #3. In order to see if there is any working problem in this board design, it is first tested with the other e-puck which uses the previous XBee Boards. <br />
<br />
The e-puck #3 upgraded with the new XBee board did not show any problem in communicating with other e-pucks. According to the goal defined, all e-pucks, including e-puck #3, locate themselves to the desired location.<br />
<br clear="all"><br />
=====Color Sensor Circuit=====<br />
{|<br />
| [[Image:color_sensor_circuit_diagram_v1_R.gif|300px|thumb|Red Color Sensor Circuit]]<br />
| [[Image:color_sensor_circuit_diagram_v1_G.gif|315px|thumb|Green Color Sensor Circuit]]<br />
| [[Image:color_sensor_circuit_diagram_v1_B.gif|300px|thumb|Blue Color Sensor Circuit]]<br />
|}<br />
<br />
As you may draw from the circuit diagrams above, as each photodiode receives light, a certain amount of current start to flow through the photodiodes and generates a voltage across R<sub>1</sub> = 680K. Each photodiode is designed to detect the certain range of wavelength of the light, and the amount of current flowing through the photodiodes is determined according to the amount of the corresponding light to each photodiode. The op-amp (LMC6484) takes the voltage generated across R<sub>1</sub> as the input signal, amplifying it by a ratio particular to the circuit. This ratio is also known as gain, and is defined by resistance of the potentiometer. The now amplified output is then sent to the analog digital converter, which on the e-Puck had been used as the X,Y, and Z axis accelerometers. This convenient, as each accelerometer axis can be used as a channel for the color sensors three colors. The converted signal can then be used to measure the response of the color sensor to light. The corresponding equation for the circuits illustrated above are as follows:<br />
<br />
<math>|V_o| = |V_i * \frac{R_2}{R_{pot}}|</math><br />
<br />
*R<sub>pot</sub> = resistance of the potentiometer (shown in the diagram)<br />
*R<sub>2</sub> = 100K (shown in the diagram)<br />
*V<sub>i</sub> = voltage across R<sub>1</sub> = 680K, which the op-amp takes as an input<br />
*V<sub>o</sub> = output signal amplified from the op-amp<br />
<br />
The gain of the color sensor circuits is approximately 20. Thus, the input voltage, V<sub>i</sub>, is amplified to be 20V<sub>i</sub>, which is V<sub>o</sub>. As mentioned above, the gain can be adjusted properly by controlling the resistance of the potentiometer.<br />
<br />
As shown in the circuit diagram on the left, the siganl from the red photodiode goes into the pin #5, and the amplified signal is sent out through the pin # 7. Similarly, the signal from the green photodiode goes into the pin #3 and it is sent out from pin #1 while the signal from the blue photodiode goes into the pin #12, and it is sent out from pin #14. <br />
<br />
Output Pins<br />
*Pin #7 - Amplified Red photodiode signal<br />
*Pin #1 - Amplified Green photodiode signal<br />
*Pin #14 - Amplified Blue photodiode signal<br />
<br />
=====Parts used=====<br />
Parts used in both the previous version and the new version of XBee Interface Extension Board<br />
*2x 10 pos. 2 mm pitch socket (Digikey S5751-10-ND) <br />
*LE-33 low dropout voltage regulator (Digikey 497-4258-1-ND) <br />
*2.2uF tantalum capacitor (Digikey 399-3536-ND) <br />
*2x Samtec BTE-020-02-L-D-A (Order directly from Samtec) <br />
*0.1"header pins for RTS and CTS pins (you can also use wire for a permanent connection) <br />
*2x 0.1" jumpers for connecting RTS and CTS pins if you used header pins(Digikey S9000-ND) <br />
<br />
Additional parts for new version of XBee Interface Extension Board<br />
*3x 100K resistors<br />
*3x 680K resistors<br />
*3x 10K potentiometer<br />
*3x 5pF capacitor<br />
*1x RGB color sensor (Order directly from HAMAMATSU, part#:s9032-02, [http://jp.hamamatsu.com/resources/products/ssd/pdf/s9032-02_kspd1067e03.pdf Datasheet])<br />
*1x High impedence op-amp LMC6484<br />
<br />
=====Future modifications=====<br />
As mentioned in the overview, the black dot patterns of e-pucks are replaced with new LED patterns by implementing LED pattern board at the top of each e-puck. Thus, in order for the color sensor to collect data properly, it is necessary to move the color sensor from the XBee Interface Extension Board to the LED pattern board so that nothing will block the color sensor. All other components for the color sensor circuit remains in the XBee Interface Extension Board and only the color sensor will be place in the LED pattern board. We can use a jumper to connect the color sensor placed at the LED pattern board to the color sensor circuit place in the XBee Interface Extension Board. The datails of this LED pattern Board will be presented at the section below.<br />
----<br />
<br />
===LED Pattern Board===<br />
[[Image:LED_pattern_board.gif|280px|right|thumb]]<br />
This is the LED pattern board, which was introduced for the RGB Swarm Robot Project. Currently, the unique black dot pattern of each e-puck was used for the machine vision system to recognize each e-puck. However, this black dot pattern requires a white background in order for the machine vision system to recognize e-pucks. The new LED pattern board uses LEDs with the proper brightness, instead of the black dot pattern. By doing so, the machine vision system can now recognize e-pucks on any background. The reason why this LED pattern is recognized on any background will be presented briefly in the Code section below. In addition, in order to apply this LED pattern to the machine vision system, we made a modification in code. This modification will also be presented in the Code Section below. The PCB file can be downloaded here: [[Media:LED_Pattern_Board.zip|'''LED Pattern Board''']]. This file contains the Traxmaker PCB files for an individual LED Pattern Board, as well as a 2x2 array, along with the necessary Gerber and drill files necessary for ordering PCBs.<br />
<br />
====LED Pattern Board Design====<br />
This LED Pattern Board is created using Traxmaker. This LED Board design can be downloaded here:<br />
Although we replaced the black dots with LEDs, we maintain each pattern of dots. The horizontal distance and the vertical distance between the two adjacent LEDs are both 0.8 inch. In order to reduce power consumption of the e-puck battery, we implement a seperate pair of AAA batteries to supply power to the LEDs. This LED board can be turned on and off by the switch.<br />
The millicandela rating of the LEDs used is 4850 mcd. In addition, this LED has diffused lens style. The reason to choose this LED is that it has a proper brightness and power consumption, and it is diffused so that the machine vision system can capture this LED in any places.The resistor used are 68.7 ohm. <br />
<br />
As mentioned in the XBee Interface Extension Board section, the color sensor has to be moved to this LED pattern board from the XBee Interface Extension Board so that nothing blocks the sensor. Thus, as you can see in the Figure on the left, the color sensor is place at the front, and each photodiode is connected to the 10 pin header. This header connects the color sensor on the LED pattern board to the remaining part of color sensor circuit on the XBee Interface Extension Board v2. <br />
<br />
====Parts used====<br />
*3x LED (Digikey 516-1697-ND): Some e-pucks require 4 LEDs since they have a pattern composed of 4 dots<br />
*3x 68.7 ohm resistors : Some e-pucks require 4 resistors since they have 4 LEDs<br />
*2x AAA Battery Holder (Digikey 2466K-ND)<br />
*1x Switch (Digikey CKN1068-ND)<br />
*1x RGB color sensor (Order directly from HAMAMATSU, part#:s9032-02)<br />
*1x 10 pos. 2 mm pitch socket (Digikey S5751-10-ND) <br />
<br />
====Tests====<br />
<br />
=====LED Distance vs Color Sensor Signal=====<br />
Tests need be done in order to note the affect of the LED light on the color sensor due to potential interference. The first experiment performed is designed to see how much interference will be caused as the distance between the LED and the color sensor changes.<br />
<br />
'''Setup and Results'''<br />
<br />
1. A white LED is used in this experiment because the white LED will cover the entire wavelengh ranges of the visible light. The experiment with the white LED can yield a general result, while the experiment with the colored LEDs will yield more specific result focused on the interference between the certain photodiode and the certain color.<br />
*LED: 18950 mcd (millicandela), digikey part number: C503B-WAN-CABBB151-ND<br />
<br />
2. The experiment was performed under the two conditions; with the ambient light and without the ambient light. <br />
<br />
3. The LED and the color sensor were placed at the same plane, and both are facing upward. <br />
<br />
4. Distance between the color sensor and the LED is increased by 0.25 inch each time from 1 inch to 2.5 inch.<br />
<br />
5. The amplified output, V<sub>o</sub> as shown in the circuit diagram above, of each photodiode is measured.<br />
<br />
With Ambient light<br />
[[Image:Distance_vs_output_with_room_light.gif|450px|left|thumb]]<br />
*Unit: Volt, V<br />
<br />
{| class="wikitable" border="3"<br />
|+'''Distance vs Amplified Output'''<br />
|-<br />
! Distance !! R !! G !! B <br />
|-<br />
| No LED|| 1 || 1.4 || 0.469<br />
|-<br />
| 1 inch || 1.259 || 1.716 || 0.832<br />
|-<br />
| 1.25 inch || 1.185 || 1.619 || 0.757<br />
|-<br />
| 1.5 inch || 1.135 || 1.529 || 0.669<br />
|-<br />
| 1.75 inch || 1.097 || 1.503 || 0.613<br />
|-<br />
| 2 inch || 1.086 || 1.481 || 0.589<br />
|-<br />
| 2.25 inch || 1.071 || 1.47 || 0.563<br />
|-<br />
| 2.5 inch || 1.06 || 1.453 || 0.546<br />
|}<br />
<br clear="all"><br />
Without the Ambient Light<br />
[[Image:Distance_vs_output_without_room_light.jpg|450px|left|thumb]]<br />
*Unit: Volt, V<br />
<br />
{| class="wikitable" border="3"<br />
|+'''Distance vs Amplified Output'''<br />
|-<br />
! Distance !! R !! G !! B <br />
|-<br />
| No LED|| 0.028 || 0.025 || 0.019<br />
|-<br />
| 1 inch || 0.244 || 0.221 || 0.223<br />
|-<br />
| 1.25 inch || 0.195 || 0.166 || 0.143<br />
|-<br />
| 1.5 inch || 0.162 || 0.123 || 0.097<br />
|-<br />
| 1.75 inch || 0.130 || 0.097 || 0.069<br />
|-<br />
| 2 inch || 0.102 || 0.077 || 0.054<br />
|-<br />
| 2.25 inch || 0.087 || 0.064 || 0.045<br />
|-<br />
| 2.5 inch || 0.073 || 0.056 || 0.039<br />
|}<br />
<br clear="all"><br />
As you can see in the two graphs above, the color sensor is affected by the light from the LED. The color sensor is most affectd by the LED when the LED is closest to it. As the distance between the LED and the color sensor increases, the interference decreases. When the color sensor is most affected by the LED under the presence of the room light, the output increases up to 25.9%, 22.6%, and 43.6 % of the original output. As the LED is 2.5 inch away from the color sensor, the output becomes very close to the original value. <br />
In this experiment, we see that the lights from LEDs can affect the color sensor. However, we used much brighter LED in this experiment than the ones we use in the LED pattern board. The brightness of the LED used in the experiment is 4 times larger than the ones in the LED pattern board. Thus, more experiment with the LEDs used in the LED pattern board is required.<br />
<br />
=====LED Angle vs Color Sensor Signal=====<br />
<br />
The second experiment is designed to see how much interference will be caused as the angle between LED and color sensor changes. Different from the first experiment, V<sub>i</sub>, the voltage before amplified, is mesured since amplified output, V<sub>o</sub>, easily reaches to the maximum.<br />
<br />
'''Setup and Results'''<br />
<br />
1. A white LED is used again in this experiment with the same reason above for the first experiment.<br />
*LED: 18950 mcd, Digikey part number: C503B-WAN-CABBB151-ND<br />
<br />
2. The experiment was performed under the two conditions; with the ambient light and without the ambient light. <br />
<br />
3. In this experiment, the distance between LED and color sensor is kept constant, 1 inch. <br />
<br />
4. Angle between LED and color sensor is increased by 15º each time from 0º to 90º.<br />
<br />
When the angle is 0º, the LED and the color sensor is placed at the same horizontal plane. The LED is facing toward the color sensor(this means that the LED is parallel to the horizontal plane with its head facing the color sensor, which is placed on the same horizontal plane), and the color sensor is facing upward. We increased the angle by 15º each time, and increasing amounts of light from the LED shines onto the color sensor. When the angle is 90º, the LED is right above the color sensor, facing the color sensor directly. This means that the LED and the color sensor are now on the same vertical line, and the LED is facing downward.<br />
<br />
5. The voltage before amplified, V<sub>i</sub> as shown in the circuit diagram above, of each photodiode is measured.<br />
* The reason to measure the volatage before amplified is that the output becomes too large after amplified.<br />
<br />
With the Ambient Light<br />
[[Image:Angle_vs_output_with_room_light.gif|450px|left|thumb]]<br />
*Unit: Volt, V<br />
<br />
{| class="wikitable" border="3"<br />
|+'''Angle vs Voltage Before Amplified'''<br />
|-<br />
! Angle !! R !! G !! B <br />
|-<br />
| 0º || 0.437 || 0.425 || 0.404<br />
|-<br />
| 15º || 0.475 || 0.470 || 0.451<br />
|-<br />
| 30º || 0.490 || 0.491 || 0.501<br />
|-<br />
| 45º || 0.505 || 0.506 || 0.520<br />
|-<br />
| 60º || 0.484 || 0.468 || 0.484<br />
|-<br />
| 75º || 0.457 || 0.453 || 0.440<br />
|-<br />
| 90º || 0.439 || 0.430 || 0.408<br />
|}<br />
<br clear="all"><br />
<br />
Without the Ambient Light<br />
[[Image:Angle_vs_output_without_room_light.jpg|450px|left|thumb]]<br />
*Unit: Volt, V<br />
<br />
{| class="wikitable" border="3"<br />
|+'''Angle vs Voltage Before Amplified'''<br />
|-<br />
! Angle !! R !! G !! B <br />
|-<br />
| 0º || 0.446 || 0.436 || 0.416<br />
|-<br />
| 15º || 0.454 || 0.491 || 0.461<br />
|-<br />
| 30º || 0.493 || 0.505 || 0.480<br />
|-<br />
| 45º || 0.512 || 0.521 || 0.520<br />
|-<br />
| 60º || 0.498 || 0.486 || 0.491<br />
|-<br />
| 75º || 0.498 || 0.492 || 0.487<br />
|-<br />
| 90º || 0.485 || 0.479 || 0.515<br />
|}<br />
<br clear="all"><br />
<br />
As the first experiment, two graph above shows that the color sensor is affected by the light from the LED. The color sensor is most affectd by the LED when the angle between two is 45º. The inteference increases as the angle goes to 45º, and reaches to the peak at 45º. Then it decreases as the angle goes to 90º. When the color sensor is most affected by the LED under the presence of the room light, the output increases upto 15.6%, 19.1%, and 28.7% of Vi. As angle becomes 90º, the output becomes very close to the value at the angle of 0º. The reason why the interference is reduced as the angle reaches 90º is that the ambient light presented are blocked by the LED board. When we perform this experiment, the LEDs are implemented on the LED plane. This LED plane blocks the light and make a shadow on the color sensor. Thus, the amount of light that the color sensor receives decreases. That is why the output becomes close to its original value while the angle increases.<br />
<br />
====Next Steps====<br />
The LED Pattern Board design above needs to be modified in the following parts.<br />
*The hole size for the LEDs has to increase so that it can accomodate the standoff of the LED chosen.<br />
*The hole size for the switch has to increase so that the switch can be completely inserted through the hole.<br />
*Currently, 10 pos 2mm pitch socket is used to connect the color sensor to the circuit using wires. Instead, the proper header for the color sensor has to be found to connect the color sensor and the circuit more conveniently.<br />
<br />
==Machine Vision Localization System Modification==<br />
Below is the documentation of changes made to the original machine vision localization system code to accommodate changed setup of the RGB Swarm Robot Project. This version of the code can be downloaded from the following link [[media:RGB_Machine_Vision_Localization_System.zip|'''RGB Machine Vision Localization System''']].<br />
<br />
===Compatibility Problem of Original Code with LEDs===<br />
The Machine Vision Localization System takes the real (color) image from the four cameras, and converts it into a grey-scale image. Then, using a threshold set in the machine vision code, the grey-scaled image is divided into black and white, and this black and white image is presented on the machine vision system computer screen. With the current set-up, the white background on the floor is presented as black, and black dot patterns on e-pucks are presented as white patterns. The system recognizes theses white dot patterns and identify e-pucks, and broadcasts the position coordinates to each e-puck via the Xbee Radio. For more information about the theory and operation of the system, look through the [[Machine Vision Localization System]] article.<br />
<br />
However, there is a problem with using black dot patterns to identify e-pucks. Since the machine vision system and code use a preset threshold to divide the grey image into black and white, black dot patterns are affected by the background color due to lack of contrast. For instance, if the background is black, or any color besides white, the system would have a difficult time distinguishing the pattern from the background, and possible not capture them at all. In addition, other problems arise from dirt and debris tracked onto the white surface of the floor, resulting in false patterns, further throwing the system.<br />
<br />
A solution is to substitute the black dots with LEDs placed atop the e-pucks, allowing the machine vision system to capture the identification pattern clearly regardless of background color and condition. By adjusting the threshold set in the machine vision code, the system will rely on the contrast of light intensity, minimizing the interference of the operating environment whose light intensity is which is naturally weaker than LEDs'. <br />
====Change from Original Code====<br />
In '''main.cpp''' in '''RGBVisionTracking.vcproj''', the RGB Vision project, the code has been changed in<br />
<br />
'''Line 56''':<br />
cvThreshold(greyImage[camerai], thresholdedImage[camerai], threshold, 255, '''CV_THRESH_BINARY_INV''');<br />
to<br />
cvThreshold(greyImage[camerai], thresholdedImage[camerai], threshold, 255, '''CV_THRESH_BINARY''');<br />
<br />
and<br />
<br />
'''Line 731''':<br />
cvThreshold(grey, thresholded_image, threshold, 255, '''CV_THRESH_BINARY_INV''');<br />
to<br />
cvThreshold(grey, thresholded_image, threshold, 255, '''CV_THRESH_BINARY''');<br />
<br />
Also, in '''global_vars.h''',<br />
<br />
'''Line 65''':<br />
double threshold = '''75'''; //black/white threshold<br />
to<br />
double threshold = '''200'''; //black/white threshold<br />
<br />
As change '''''CV_THRESH_BINARY_INV''''' in both line 48 and 735 to '''''CV_THRESH_BINARY''''' and adjust the value of threshold from '''''75''''' to '''''200''''', the system now clearly presents LED patterns as white dot patterns on the screen, so it can identify e-pucks according to LED patterns.<br />
<br />
====Threshold Testing====<br />
The threshold value of ''200'' is determined to be good enough for the test inside. With various conditions, however, the threshold value can, or should, be changed more properly. In addition, the results for different range of threshold under the same test condition is presented below:<br />
{| class="wikitable" border="3"<br />
|+'''Threshold range'''<br />
|-<br />
! Range !! Result <br />
|-<br />
| 0 - 94 || System cannot caputure LED patterns at all; whole screen is white.<br />
|-<br />
| 95 - 170 || System can recognize the pattern but it is unstable, since most of background becomes white.<br />
|-<br />
| 171 - 252 || System cleary captures and recognizes LED patterns.<br />
|-<br />
| 253|| System can recognize the pattern but it is unstable since pattern is too small; stronger intensity is required.<br />
|-<br />
| 254 - 255 || System cannot caputure LED patterns at all; whole screen is black.<br />
|}<br />
<br />
<br />
An e-puck was fitted with a LED pattern board and then tested with the machine vision localization system. With the changes implemented, the machine vision localization system did not show any problems, showing the ability to capture and locate the e-puck located in anywhere in the field of vision of the cameras. In addition, the vision system was able to capture and locate the e-puck as it moved. There was no loss of positional accuracy as compared to previous implementations of identification systems. The recognition of the e-puck by the machine vision localization system displayed the stability of the LED boards with the vision system, further supporting their implementation for further experiments.<br />
<br />
===Center of Mass Problem with LEDs===<br />
Another problem with the implementation of the LED pattern boards is related to the method that the machine vision localization system generates a position of a puck. Originally, the vision system determines the center of the mass of the paper dice dot patterns ('''R<sub>dots</sub>'''), and calculates the world position coordinate using the calibration information. From this, the vision system then assigns '''R<sub>dots</sub>''' as the center of mass of the e-puck ('''R<sub>e-puck</sub>'''). While '''R<sub>dots</sub>''' is typically not located over '''R<sub>e-puck</sub>''', the paper dots location was shifted in order to match '''R<sub>dots</sub>''' with '''R<sub>e-puck</sub>'''.<br />
<br />
However, this is not achievable with the LED pattern boards. As LED pattern board's location on the e-puck cannot be shifted, the vision system needs to be able to shift '''R<sub>LEDs</sub>''' to accommodate for this position error. By augmenting the '''target_classifiers.txt''' file with additional data, the modified vision system refers to a look-up table in order to determine the amount and direction that the coordinates of center of mass of the LED dots ('''R<sub>LEDs</sub>''') needs to be shifted to align with '''R<sub>e-puck</sub>'''. The look-up table contains values that are unique and constant to each rotationally invariant pattern, and as a result, only a few simple calculations are needed to generate correct coordinates for the e-puck.<br />
<br />
The result is the vision system yields a much more accurate position data for the e-puck. A simple test, which consists of rotating the e-puck around the '''R<sub>e-puck</sub>''' shows that the coordinate data for '''R<sub>e-puck</sub>''' varies by roughly 4-5mm when using the RGB modified code. Using the original Machine Vision Localization System code, the coordinate data for '''R<sub>e-puck</sub>''' varies by much more, between 40-50mm. Note that the only difference between these two tests, is the application of code to shift the '''R<sub>e-puck</sub>''' back into place; otherwise both sets of code are changed to accommodate the LED pattern boards, which were used in this test.<br />
<br />
====Addition to the Original Code====<br />
There are only a few minor changes in the code from the original machine vision code to the RGB machine vision code. The change focused around the introduction of '''centermag''' and '''centeroffset''', two new variables. '''Centermag''' refers to the magnitude from the vision system calculated '''R<sub>dots</sub>''' to the desired '''R<sub>e-puck</sub>'''. This value differs from e-puck to e-puck, the the value remains constant for each e-puck regardless of orientation. '''Centeroffset''' refers to the angle offset between the vision system calculated angle, and the line from '''R<sub>dots</sub>''' to the desired '''R<sub>e-puck</sub>'''. Again, this value differs from e-puck to e-puck, and also remains constant for each e-puck regardless of orientation.<br />
<br />
The actual code which shifts the coordinate data from '''R<sub>dots</sub>''' to the desired '''R<sub>e-puck</sub>''' is simple.<br />
<br />
In '''main.cpp''' of '''RGBVisionTracking.proj''', the code is as follows:<br />
<br />
line '''1082''' through line '''1109'''<br />
targets_temp[camerai]->group.wx = targets_temp[camerai]->group.wx + -1*targets_temp[camerai]->centermag*cos(Adjusted);<br />
targets_temp[camerai]->group.wy = targets_temp[camerai]->group.wy + -1*targets_temp[camerai]->centermag*sin(Adjusted);<br />
<br />
'''Adjusted''' is the center offset angle plus the orientation angle, or '''centeroffset + angle''', which results is the angle of the line from '''R<sub>dots</sub>''' to '''R<sub>e-puck</sub>''', from zero degrees. With knowledge of this angle, a simply trigonometry problem is performed to determine the x and y values to add/subtract from '''R<sub>dots</sub>''' to get coordinates for '''R<sub>e-puck</sub>'''.<br />
<br />
The values for centermag and centeroffset are found in the txt file, '''target_classifiers_augmented''', as the fourth and fifth values, respectively. This file can be round in the modified code zip file above.<br />
<br />
===Other Considerations===<br />
While there do not need to be any additional changes to the set up of the machine vision localization system, there may be additional considerations for further development. One such consideration is the 'background', or floor material, of the setup. With the modified machine vision code, light intensity is what is picked up and filtered by the system, thus rendering the LEDs from the e-pucks to be the only tracked objects. However, with more advanced set ups, such as one featuring light that is projected onto the background, this may present a problem with the machine vision system picking up reflected light. More testing has to be done with modifying the machine vision system threshold to see if there is an ideal threshold to accommodate this setup. Another option may be to use a non-reflective or matte surface for the background.<br />
<br />
Another consideration involves the hardware of the setup, or the themselves. The cameras are equipped with the Logitech software which automatically adjusts the exposure and light contrast settings to correct for poor lighting and setup conditions. However, this leads to issues as with increased exposure (due to light blocking set up) and bright LEDs results in blurry or blobby images received. The machine vision localization system cannot read these images, and as a result cannot track the e-pucks. One potential solution may be to adjust the threshold of the vision system. Other solutions may be to use LEDs with a lower millicandela, or increase the background lighting of the setup, for instance with upward casting lights.<br />
<br />
==RGB Swarm E-puck Code Overview==<br />
Detailed code outline: [[Swarm RGB E-puck Code]]<br />
<br />
==MATLAB Code for RGB==<br />
<br />
[[Media:RGB_Swarm_MATLAB_09-03-09.zip]]<br />
<br />
====What to do with the files:====<br />
* '''open_serial.m:''' Change the COM call to whatever COM port your XBee radio is plugged into (COM1, COM2, ...). Run this to open the COM port before you run anything else. It will create a COM object in your workspace. It takes a long time, be patient.<br />
* '''close_serial.m:''' Run after you're done to close the COM port. Keeps matlab and your computer happy.<br />
* '''RGB_Swarm_Data_Grabber.m:''' Use this to plot the values from the RGB sensor of one puck. It shows a moving plot. Handy for testing.<br />
* '''RGB_avg_std_logger.m:''' Interactive point logger. Follow the prompts to grab some nice data. Useful for testing.<br />
* '''color_hist.m:''' Plot a histogram from RGB_Swarm_Data_Grabber.<br />
<br />
==Physical Setup==<br />
In the RGB swarm robot project, the epucks pick up light from a projector. This projector has to project onto the floor so that the top mounted light sensors can pick up the light. The floor which the epucks roll on must be completely enclosed so that the only light which reaches it, is the light from the projector. Also this floor must be smooth, flat and durable. See the overhead view below.<br />
<br />
{| align="left" cellpadding = "25" <br />
! [[Image:RGBswarmsetup.jpg|600px|center]]<br />
|}<br />
<br />
<br clear=all><br />
<br />
===Curtains===<br />
The floor is enclosed by two walls and 6 curtains. Two bars protrude from the walls and are connected by an L-joint. There are 3 Eclipse absolute zero curtains on each bar (see diagram). These curtains block 100% light and are sewn together so that no light comes through between them. Covering the whole enclosure, above the projector mount are 7 more curtains sewn together to block all light. <br />
<br />
<br />
<br />
===Floor===<br />
The floor is currently a vinyl sheet from home depot. The sheet had a raised pattern on the front, so it was flipped over. The underside of the vinyl floor is not very durable, but holds up as long as no shoes are used when stepping on it. In the future, a more durable light colored floor may want to be used. Also, the floor is taped down with packing tape. The tape should not be removed as the floor has markings which show where the camera calibration dots should go and where the projected image should fall.<br />
<br />
<br />
<br />
===Projector===<br />
The projector is a Benq MP771 DLP prpjector. It has a digital user manual on a CD in the projection computer. <br />
<br />
Since it is DLP, it has an array of tiny mirrors which reflect the light from the bulb. The light from the bulb is shown through a color wheel which shines red, green, and blue on to the mirror array. The frequency with which the mirrors turn on and off (reflect light and don't) determines the intensity of light. For example if a dark red was being projected, the mirrors would be on more than off in a certain interval. In the case of our projector that interval is %%??8.2 millisecond??%%.<br />
<br />
Each mirror represents a different pixel projected from the projector. This projector has a resolution of ?1024 x 760?, so in order to get a 1 to 1 pixel ratio, the projection computer should be set to display at 1024 x 760. <br />
<br />
The projector should not be tilted forward or backward more that 15°. Because of this and the wide throw of the projector, a keystone projection shape could not be avoided. The projector is programmed to project at the maximum amount of keystone.<br />
<br />
The size of the projected image....<br />
<br />
<br />
<br />
===Projector Mount===<br />
Etiam a odio sit amet nunc ullamcorper fringilla non id dui. Sed interdum, ligula pretium aliquet ornare, ligula metus sodales nisi, in fermentum erat purus id diam. Suspendisse tincidunt scelerisque enim, vitae hendrerit nunc suscipit ac. Cras luctus, nisl eu congue sagittis, nibh quam ultricies eros, vitae ultricies odio mauris vitae urna. Vestibulum ante ipsum primis in faucibus orci luctus et ultrices posuere cubilia Curae; Nam in felis cursus urna pretium scelerisque. Morbi lacinia semper venenatis. Integer pharetra risus et purus luctus pellentesque. Nam risus justo, consectetur nec vestibulum eu, molestie eget felis. Sed tristique congue nulla eget luctus. In suscipit dignissim mollis. Morbi ac pellentesque lacus. Sed vel mauris at nulla imperdiet hendrerit id et sem. Sed elementum semper quam sed feugiat. Suspendisse quis felis vitae sapien placerat aliquet nec quis mauris. Vivamus eget auctor diam. Suspendisse convallis leo nec turpis iaculis quis fringilla libero hendrerit.<br />
<br />
=='''Conclusion'''==<br />
<br />
The new XBee Interface Extension Board design was tested, and we found out that it does not have any problem. In addition, the black dot pattern of the e-pucks are upgraded to LED patterns. The advantage of this improvement is that the machine vision system can recoginize each e-puck no matter where the e-pucks are located. The color of the background also does not affect the vision system. However, we had to move the color sensor to the LED pattern board since the LED pattern board will block the sensor if the sensor is located in the XBee Interface Extension Board. Thus, we now consider the light interference between the LEDs and the color sensor. In the light interference test, we found out that the color sensor is affected by the light from LED. However, since we used much brighter LEDs in our light interference test than the LEDs used for the LED pattern board, we have to do more experiment on this in order to have more accurate interference data. <br />
<br />
<br />
[[Category:SwarmRobotProject]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_Documentation&diff=14268RGB Swarm Robot Project Documentation2009-09-04T17:38:47Z<p>Sam Bobb: /* RGB Swarm E-puck Code Overview */</p>
<hr />
<div>== Overview ==<br />
The swarm robot project has gone through several phases, with each phase focusing on different aspects of swarm robotics and the implementation of the project. This entry focuses on the most recent phase of the project, covering topics such as, but not limited to, '''Xbee Interface Extension Boards''', '''LED light boards''', and '''changes made to the Machine Vision Localization System''', and the overall conversion to LED boards and a controlled light environment. These entries help provide insight into setup and specific details to allow others to replicate or reproduce our results, and to provide additional information for those working on similar projects or this project at a later time. Other articles in the '''Swarm Robot Project''' category focus on topics such as the swarm theory and algorithms implemented, as well as previous phases of the project, such as motion control and consensus estimation. You may reach these articles and others by following the category link at the bottom of every page, or through this link - [[:Category:SwarmRobotProject|'''Swarm Robot Project''']].<br />
<br />
== Hardware ==<br />
<br />
<br />
<br />
===XBee Interface Extension Board Version 2===<br />
<br />
{|<br />
| [[Image:XBee_interface_extenstion_board_v1.gif|250px|thumb|alt=Traxmaker Image of the Previous Xbee Extension Board|Xbee Interface Extension Board Version]]<br />
| [[Image:IMG 1390-1-.jpg|300px|thumb|alt=Image of an e-Puck with the RGB Xbee Extension Board|e-Puck with previous board ]]<br />
| [[Image:XBee_interface_extenstion_board_v2.gif|vertical|250px|thumb|alt=Traxmaker Image of the Xbee Interface Exension Board Version 2|Xbee Interface Extension Board Version 2]]<br />
| <br />
|}<br />
<br />
====Previous Version====<br />
<br />
The previous version of XBee Interface Extension Board, designed by Michael Hwang.<br />
Its configuration is shown in the figure on the left, with an actual image of the board mounted on an e-Puck seen in the figure in the center. This version of the XBee Interface Board does not contain a color sensor in it. Details about this version of XBee Interface Extension Board, such as parts used and Traxmaker files can be found on the [[Swarm_Robot_Project_Documentation#Current_Version|Swarm Robot Project Documentation page]].<br />
<br clear="all"><br />
<br />
====Version 2====<br />
Th is the updated version of the Xbee board, or XBee Interface Extension Board Version 2. It is designed by Michael Hwang to accommodate further projects in the Swarm Robot Project. For this reason, the Xbee Interface Extension Board Version 2 has a color sensor circuit built in. The details of the color sensor circuit can be found in the color sensor section below. A copy of the Traxmaker PCB file for the Xbee Board Version 2 can be found at [[Media:epuck_xbee_board_v2.zip|'''Xbee Interface Extension Board Version 2''']].<br />
<br />
The RTS flow control line on the XBee is connected to the sel3 line of the e-puck. Although the CTS line is not connected to the sel2 pin in this board design, it can be easily connected with a jumper. <br />
<br />
The XBee Interface Extension Board Version 2 design was actually built and implemented on the e-puck #3. In order to see if there is any working problem in this board design, it is first tested with the other e-puck which uses the previous XBee Boards. <br />
<br />
The e-puck #3 upgraded with the new XBee board did not show any problem in communicating with other e-pucks. According to the goal defined, all e-pucks, including e-puck #3, locate themselves to the desired location.<br />
<br clear="all"><br />
=====Color Sensor Circuit=====<br />
{|<br />
| [[Image:color_sensor_circuit_diagram_v1_R.gif|300px|thumb|Red Color Sensor Circuit]]<br />
| [[Image:color_sensor_circuit_diagram_v1_G.gif|315px|thumb|Green Color Sensor Circuit]]<br />
| [[Image:color_sensor_circuit_diagram_v1_B.gif|300px|thumb|Blue Color Sensor Circuit]]<br />
|}<br />
<br />
As you may draw from the circuit diagrams above, as each photodiode receives light, a certain amount of current start to flow through the photodiodes and generates a voltage across R<sub>1</sub> = 680K. Each photodiode is designed to detect the certain range of wavelength of the light, and the amount of current flowing through the photodiodes is determined according to the amount of the corresponding light to each photodiode. The op-amp (LMC6484) takes the voltage generated across R<sub>1</sub> as the input signal, amplifying it by a ratio particular to the circuit. This ratio is also known as gain, and is defined by resistance of the potentiometer. The now amplified output is then sent to the analog digital converter, which on the e-Puck had been used as the X,Y, and Z axis accelerometers. This convenient, as each accelerometer axis can be used as a channel for the color sensors three colors. The converted signal can then be used to measure the response of the color sensor to light. The corresponding equation for the circuits illustrated above are as follows:<br />
<br />
<math>|V_o| = |V_i * \frac{R_2}{R_{pot}}|</math><br />
<br />
*R<sub>pot</sub> = resistance of the potentiometer (shown in the diagram)<br />
*R<sub>2</sub> = 100K (shown in the diagram)<br />
*V<sub>i</sub> = voltage across R<sub>1</sub> = 680K, which the op-amp takes as an input<br />
*V<sub>o</sub> = output signal amplified from the op-amp<br />
<br />
The gain of the color sensor circuits is approximately 20. Thus, the input voltage, V<sub>i</sub>, is amplified to be 20V<sub>i</sub>, which is V<sub>o</sub>. As mentioned above, the gain can be adjusted properly by controlling the resistance of the potentiometer.<br />
<br />
As shown in the circuit diagram on the left, the siganl from the red photodiode goes into the pin #5, and the amplified signal is sent out through the pin # 7. Similarly, the signal from the green photodiode goes into the pin #3 and it is sent out from pin #1 while the signal from the blue photodiode goes into the pin #12, and it is sent out from pin #14. <br />
<br />
Output Pins<br />
*Pin #7 - Amplified Red photodiode signal<br />
*Pin #1 - Amplified Green photodiode signal<br />
*Pin #14 - Amplified Blue photodiode signal<br />
<br />
=====Parts used=====<br />
Parts used in both the previous version and the new version of XBee Interface Extension Board<br />
*2x 10 pos. 2 mm pitch socket (Digikey S5751-10-ND) <br />
*LE-33 low dropout voltage regulator (Digikey 497-4258-1-ND) <br />
*2.2uF tantalum capacitor (Digikey 399-3536-ND) <br />
*2x Samtec BTE-020-02-L-D-A (Order directly from Samtec) <br />
*0.1"header pins for RTS and CTS pins (you can also use wire for a permanent connection) <br />
*2x 0.1" jumpers for connecting RTS and CTS pins if you used header pins(Digikey S9000-ND) <br />
<br />
Additional parts for new version of XBee Interface Extension Board<br />
*3x 100K resistors<br />
*3x 680K resistors<br />
*3x 10K potentiometer<br />
*3x 5pF capacitor<br />
*1x RGB color sensor (Order directly from HAMAMATSU, part#:s9032-02, [http://jp.hamamatsu.com/resources/products/ssd/pdf/s9032-02_kspd1067e03.pdf Datasheet])<br />
*1x High impedence op-amp LMC6484<br />
<br />
=====Future modifications=====<br />
As mentioned in the overview, the black dot patterns of e-pucks are replaced with new LED patterns by implementing LED pattern board at the top of each e-puck. Thus, in order for the color sensor to collect data properly, it is necessary to move the color sensor from the XBee Interface Extension Board to the LED pattern board so that nothing will block the color sensor. All other components for the color sensor circuit remains in the XBee Interface Extension Board and only the color sensor will be place in the LED pattern board. We can use a jumper to connect the color sensor placed at the LED pattern board to the color sensor circuit place in the XBee Interface Extension Board. The datails of this LED pattern Board will be presented at the section below.<br />
----<br />
<br />
===LED Pattern Board===<br />
[[Image:LED_pattern_board.gif|280px|right|thumb]]<br />
This is the LED pattern board, which was introduced for the RGB Swarm Robot Project. Currently, the unique black dot pattern of each e-puck was used for the machine vision system to recognize each e-puck. However, this black dot pattern requires a white background in order for the machine vision system to recognize e-pucks. The new LED pattern board uses LEDs with the proper brightness, instead of the black dot pattern. By doing so, the machine vision system can now recognize e-pucks on any background. The reason why this LED pattern is recognized on any background will be presented briefly in the Code section below. In addition, in order to apply this LED pattern to the machine vision system, we made a modification in code. This modification will also be presented in the Code Section below. The PCB file can be downloaded here: [[Media:LED_Pattern_Board.zip|'''LED Pattern Board''']]. This file contains the Traxmaker PCB files for an individual LED Pattern Board, as well as a 2x2 array, along with the necessary Gerber and drill files necessary for ordering PCBs.<br />
<br />
====LED Pattern Board Design====<br />
This LED Pattern Board is created using Traxmaker. This LED Board design can be downloaded here:<br />
Although we replaced the black dots with LEDs, we maintain each pattern of dots. The horizontal distance and the vertical distance between the two adjacent LEDs are both 0.8 inch. In order to reduce power consumption of the e-puck battery, we implement a seperate pair of AAA batteries to supply power to the LEDs. This LED board can be turned on and off by the switch.<br />
The millicandela rating of the LEDs used is 4850 mcd. In addition, this LED has diffused lens style. The reason to choose this LED is that it has a proper brightness and power consumption, and it is diffused so that the machine vision system can capture this LED in any places.The resistor used are 68.7 ohm. <br />
<br />
As mentioned in the XBee Interface Extension Board section, the color sensor has to be moved to this LED pattern board from the XBee Interface Extension Board so that nothing blocks the sensor. Thus, as you can see in the Figure on the left, the color sensor is place at the front, and each photodiode is connected to the 10 pin header. This header connects the color sensor on the LED pattern board to the remaining part of color sensor circuit on the XBee Interface Extension Board v2. <br />
<br />
====Parts used====<br />
*3x LED (Digikey 516-1697-ND): Some e-pucks require 4 LEDs since they have a pattern composed of 4 dots<br />
*3x 68.7 ohm resistors : Some e-pucks require 4 resistors since they have 4 LEDs<br />
*2x AAA Battery Holder (Digikey 2466K-ND)<br />
*1x Switch (Digikey CKN1068-ND)<br />
*1x RGB color sensor (Order directly from HAMAMATSU, part#:s9032-02)<br />
*1x 10 pos. 2 mm pitch socket (Digikey S5751-10-ND) <br />
<br />
====Tests====<br />
<br />
=====LED Distance vs Color Sensor Signal=====<br />
Tests need be done in order to note the affect of the LED light on the color sensor due to potential interference. The first experiment performed is designed to see how much interference will be caused as the distance between the LED and the color sensor changes.<br />
<br />
'''Setup and Results'''<br />
<br />
1. A white LED is used in this experiment because the white LED will cover the entire wavelengh ranges of the visible light. The experiment with the white LED can yield a general result, while the experiment with the colored LEDs will yield more specific result focused on the interference between the certain photodiode and the certain color.<br />
*LED: 18950 mcd (millicandela), digikey part number: C503B-WAN-CABBB151-ND<br />
<br />
2. The experiment was performed under the two conditions; with the ambient light and without the ambient light. <br />
<br />
3. The LED and the color sensor were placed at the same plane, and both are facing upward. <br />
<br />
4. Distance between the color sensor and the LED is increased by 0.25 inch each time from 1 inch to 2.5 inch.<br />
<br />
5. The amplified output, V<sub>o</sub> as shown in the circuit diagram above, of each photodiode is measured.<br />
<br />
With Ambient light<br />
[[Image:Distance_vs_output_with_room_light.gif|450px|left|thumb]]<br />
*Unit: Volt, V<br />
<br />
{| class="wikitable" border="3"<br />
|+'''Distance vs Amplified Output'''<br />
|-<br />
! Distance !! R !! G !! B <br />
|-<br />
| No LED|| 1 || 1.4 || 0.469<br />
|-<br />
| 1 inch || 1.259 || 1.716 || 0.832<br />
|-<br />
| 1.25 inch || 1.185 || 1.619 || 0.757<br />
|-<br />
| 1.5 inch || 1.135 || 1.529 || 0.669<br />
|-<br />
| 1.75 inch || 1.097 || 1.503 || 0.613<br />
|-<br />
| 2 inch || 1.086 || 1.481 || 0.589<br />
|-<br />
| 2.25 inch || 1.071 || 1.47 || 0.563<br />
|-<br />
| 2.5 inch || 1.06 || 1.453 || 0.546<br />
|}<br />
<br clear="all"><br />
Without the Ambient Light<br />
[[Image:Distance_vs_output_without_room_light.jpg|450px|left|thumb]]<br />
*Unit: Volt, V<br />
<br />
{| class="wikitable" border="3"<br />
|+'''Distance vs Amplified Output'''<br />
|-<br />
! Distance !! R !! G !! B <br />
|-<br />
| No LED|| 0.028 || 0.025 || 0.019<br />
|-<br />
| 1 inch || 0.244 || 0.221 || 0.223<br />
|-<br />
| 1.25 inch || 0.195 || 0.166 || 0.143<br />
|-<br />
| 1.5 inch || 0.162 || 0.123 || 0.097<br />
|-<br />
| 1.75 inch || 0.130 || 0.097 || 0.069<br />
|-<br />
| 2 inch || 0.102 || 0.077 || 0.054<br />
|-<br />
| 2.25 inch || 0.087 || 0.064 || 0.045<br />
|-<br />
| 2.5 inch || 0.073 || 0.056 || 0.039<br />
|}<br />
<br clear="all"><br />
As you can see in the two graphs above, the color sensor is affected by the light from the LED. The color sensor is most affectd by the LED when the LED is closest to it. As the distance between the LED and the color sensor increases, the interference decreases. When the color sensor is most affected by the LED under the presence of the room light, the output increases up to 25.9%, 22.6%, and 43.6 % of the original output. As the LED is 2.5 inch away from the color sensor, the output becomes very close to the original value. <br />
In this experiment, we see that the lights from LEDs can affect the color sensor. However, we used much brighter LED in this experiment than the ones we use in the LED pattern board. The brightness of the LED used in the experiment is 4 times larger than the ones in the LED pattern board. Thus, more experiment with the LEDs used in the LED pattern board is required.<br />
<br />
=====LED Angle vs Color Sensor Signal=====<br />
<br />
The second experiment is designed to see how much interference will be caused as the angle between LED and color sensor changes. Different from the first experiment, V<sub>i</sub>, the voltage before amplified, is mesured since amplified output, V<sub>o</sub>, easily reaches to the maximum.<br />
<br />
'''Setup and Results'''<br />
<br />
1. A white LED is used again in this experiment with the same reason above for the first experiment.<br />
*LED: 18950 mcd, Digikey part number: C503B-WAN-CABBB151-ND<br />
<br />
2. The experiment was performed under the two conditions; with the ambient light and without the ambient light. <br />
<br />
3. In this experiment, the distance between LED and color sensor is kept constant, 1 inch. <br />
<br />
4. Angle between LED and color sensor is increased by 15º each time from 0º to 90º.<br />
<br />
When the angle is 0º, the LED and the color sensor is placed at the same horizontal plane. The LED is facing toward the color sensor(this means that the LED is parallel to the horizontal plane with its head facing the color sensor, which is placed on the same horizontal plane), and the color sensor is facing upward. We increased the angle by 15º each time, and increasing amounts of light from the LED shines onto the color sensor. When the angle is 90º, the LED is right above the color sensor, facing the color sensor directly. This means that the LED and the color sensor are now on the same vertical line, and the LED is facing downward.<br />
<br />
5. The voltage before amplified, V<sub>i</sub> as shown in the circuit diagram above, of each photodiode is measured.<br />
* The reason to measure the volatage before amplified is that the output becomes too large after amplified.<br />
<br />
With the Ambient Light<br />
[[Image:Angle_vs_output_with_room_light.gif|450px|left|thumb]]<br />
*Unit: Volt, V<br />
<br />
{| class="wikitable" border="3"<br />
|+'''Angle vs Voltage Before Amplified'''<br />
|-<br />
! Angle !! R !! G !! B <br />
|-<br />
| 0º || 0.437 || 0.425 || 0.404<br />
|-<br />
| 15º || 0.475 || 0.470 || 0.451<br />
|-<br />
| 30º || 0.490 || 0.491 || 0.501<br />
|-<br />
| 45º || 0.505 || 0.506 || 0.520<br />
|-<br />
| 60º || 0.484 || 0.468 || 0.484<br />
|-<br />
| 75º || 0.457 || 0.453 || 0.440<br />
|-<br />
| 90º || 0.439 || 0.430 || 0.408<br />
|}<br />
<br clear="all"><br />
<br />
Without the Ambient Light<br />
[[Image:Angle_vs_output_without_room_light.jpg|450px|left|thumb]]<br />
*Unit: Volt, V<br />
<br />
{| class="wikitable" border="3"<br />
|+'''Angle vs Voltage Before Amplified'''<br />
|-<br />
! Angle !! R !! G !! B <br />
|-<br />
| 0º || 0.446 || 0.436 || 0.416<br />
|-<br />
| 15º || 0.454 || 0.491 || 0.461<br />
|-<br />
| 30º || 0.493 || 0.505 || 0.480<br />
|-<br />
| 45º || 0.512 || 0.521 || 0.520<br />
|-<br />
| 60º || 0.498 || 0.486 || 0.491<br />
|-<br />
| 75º || 0.498 || 0.492 || 0.487<br />
|-<br />
| 90º || 0.485 || 0.479 || 0.515<br />
|}<br />
<br clear="all"><br />
<br />
As the first experiment, two graph above shows that the color sensor is affected by the light from the LED. The color sensor is most affectd by the LED when the angle between two is 45º. The inteference increases as the angle goes to 45º, and reaches to the peak at 45º. Then it decreases as the angle goes to 90º. When the color sensor is most affected by the LED under the presence of the room light, the output increases upto 15.6%, 19.1%, and 28.7% of Vi. As angle becomes 90º, the output becomes very close to the value at the angle of 0º. The reason why the interference is reduced as the angle reaches 90º is that the ambient light presented are blocked by the LED board. When we perform this experiment, the LEDs are implemented on the LED plane. This LED plane blocks the light and make a shadow on the color sensor. Thus, the amount of light that the color sensor receives decreases. That is why the output becomes close to its original value while the angle increases.<br />
<br />
====Next Steps====<br />
The LED Pattern Board design above needs to be modified in the following parts.<br />
*The hole size for the LEDs has to increase so that it can accomodate the standoff of the LED chosen.<br />
*The hole size for the switch has to increase so that the switch can be completely inserted through the hole.<br />
*Currently, 10 pos 2mm pitch socket is used to connect the color sensor to the circuit using wires. Instead, the proper header for the color sensor has to be found to connect the color sensor and the circuit more conveniently.<br />
<br />
==Machine Vision Localization System Modification==<br />
Below is the documentation of changes made to the original machine vision localization system code to accommodate changed setup of the RGB Swarm Robot Project. This version of the code can be downloaded from the following link [[media:RGB_Machine_Vision_Localization_System.zip|'''RGB Machine Vision Localization System''']].<br />
<br />
===Compatibility Problem of Original Code with LEDs===<br />
The Machine Vision Localization System takes the real (color) image from the four cameras, and converts it into a grey-scale image. Then, using a threshold set in the machine vision code, the grey-scaled image is divided into black and white, and this black and white image is presented on the machine vision system computer screen. With the current set-up, the white background on the floor is presented as black, and black dot patterns on e-pucks are presented as white patterns. The system recognizes theses white dot patterns and identify e-pucks, and broadcasts the position coordinates to each e-puck via the Xbee Radio. For more information about the theory and operation of the system, look through the [[Machine Vision Localization System]] article.<br />
<br />
However, there is a problem with using black dot patterns to identify e-pucks. Since the machine vision system and code use a preset threshold to divide the grey image into black and white, black dot patterns are affected by the background color due to lack of contrast. For instance, if the background is black, or any color besides white, the system would have a difficult time distinguishing the pattern from the background, and possible not capture them at all. In addition, other problems arise from dirt and debris tracked onto the white surface of the floor, resulting in false patterns, further throwing the system.<br />
<br />
A solution is to substitute the black dots with LEDs placed atop the e-pucks, allowing the machine vision system to capture the identification pattern clearly regardless of background color and condition. By adjusting the threshold set in the machine vision code, the system will rely on the contrast of light intensity, minimizing the interference of the operating environment whose light intensity is which is naturally weaker than LEDs'. <br />
====Change from Original Code====<br />
In '''main.cpp''' in '''RGBVisionTracking.vcproj''', the RGB Vision project, the code has been changed in<br />
<br />
'''Line 56''':<br />
cvThreshold(greyImage[camerai], thresholdedImage[camerai], threshold, 255, '''CV_THRESH_BINARY_INV''');<br />
to<br />
cvThreshold(greyImage[camerai], thresholdedImage[camerai], threshold, 255, '''CV_THRESH_BINARY''');<br />
<br />
and<br />
<br />
'''Line 731''':<br />
cvThreshold(grey, thresholded_image, threshold, 255, '''CV_THRESH_BINARY_INV''');<br />
to<br />
cvThreshold(grey, thresholded_image, threshold, 255, '''CV_THRESH_BINARY''');<br />
<br />
Also, in '''global_vars.h''',<br />
<br />
'''Line 65''':<br />
double threshold = '''75'''; //black/white threshold<br />
to<br />
double threshold = '''200'''; //black/white threshold<br />
<br />
As change '''''CV_THRESH_BINARY_INV''''' in both line 48 and 735 to '''''CV_THRESH_BINARY''''' and adjust the value of threshold from '''''75''''' to '''''200''''', the system now clearly presents LED patterns as white dot patterns on the screen, so it can identify e-pucks according to LED patterns.<br />
<br />
====Threshold Testing====<br />
The threshold value of ''200'' is determined to be good enough for the test inside. With various conditions, however, the threshold value can, or should, be changed more properly. In addition, the results for different range of threshold under the same test condition is presented below:<br />
{| class="wikitable" border="3"<br />
|+'''Threshold range'''<br />
|-<br />
! Range !! Result <br />
|-<br />
| 0 - 94 || System cannot caputure LED patterns at all; whole screen is white.<br />
|-<br />
| 95 - 170 || System can recognize the pattern but it is unstable, since most of background becomes white.<br />
|-<br />
| 171 - 252 || System cleary captures and recognizes LED patterns.<br />
|-<br />
| 253|| System can recognize the pattern but it is unstable since pattern is too small; stronger intensity is required.<br />
|-<br />
| 254 - 255 || System cannot caputure LED patterns at all; whole screen is black.<br />
|}<br />
<br />
<br />
An e-puck was fitted with a LED pattern board and then tested with the machine vision localization system. With the changes implemented, the machine vision localization system did not show any problems, showing the ability to capture and locate the e-puck located in anywhere in the field of vision of the cameras. In addition, the vision system was able to capture and locate the e-puck as it moved. There was no loss of positional accuracy as compared to previous implementations of identification systems. The recognition of the e-puck by the machine vision localization system displayed the stability of the LED boards with the vision system, further supporting their implementation for further experiments.<br />
<br />
===Center of Mass Problem with LEDs===<br />
Another problem with the implementation of the LED pattern boards is related to the method that the machine vision localization system generates a position of a puck. Originally, the vision system determines the center of the mass of the paper dice dot patterns ('''R<sub>dots</sub>'''), and calculates the world position coordinate using the calibration information. From this, the vision system then assigns '''R<sub>dots</sub>''' as the center of mass of the e-puck ('''R<sub>e-puck</sub>'''). While '''R<sub>dots</sub>''' is typically not located over '''R<sub>e-puck</sub>''', the paper dots location was shifted in order to match '''R<sub>dots</sub>''' with '''R<sub>e-puck</sub>'''.<br />
<br />
However, this is not achievable with the LED pattern boards. As LED pattern board's location on the e-puck cannot be shifted, the vision system needs to be able to shift '''R<sub>LEDs</sub>''' to accommodate for this position error. By augmenting the '''target_classifiers.txt''' file with additional data, the modified vision system refers to a look-up table in order to determine the amount and direction that the coordinates of center of mass of the LED dots ('''R<sub>LEDs</sub>''') needs to be shifted to align with '''R<sub>e-puck</sub>'''. The look-up table contains values that are unique and constant to each rotationally invariant pattern, and as a result, only a few simple calculations are needed to generate correct coordinates for the e-puck.<br />
<br />
The result is the vision system yields a much more accurate position data for the e-puck. A simple test, which consists of rotating the e-puck around the '''R<sub>e-puck</sub>''' shows that the coordinate data for '''R<sub>e-puck</sub>''' varies by roughly 4-5mm when using the RGB modified code. Using the original Machine Vision Localization System code, the coordinate data for '''R<sub>e-puck</sub>''' varies by much more, between 40-50mm. Note that the only difference between these two tests, is the application of code to shift the '''R<sub>e-puck</sub>''' back into place; otherwise both sets of code are changed to accommodate the LED pattern boards, which were used in this test.<br />
<br />
====Addition to the Original Code====<br />
There are only a few minor changes in the code from the original machine vision code to the RGB machine vision code. The change focused around the introduction of '''centermag''' and '''centeroffset''', two new variables. '''Centermag''' refers to the magnitude from the vision system calculated '''R<sub>dots</sub>''' to the desired '''R<sub>e-puck</sub>'''. This value differs from e-puck to e-puck, the the value remains constant for each e-puck regardless of orientation. '''Centeroffset''' refers to the angle offset between the vision system calculated angle, and the line from '''R<sub>dots</sub>''' to the desired '''R<sub>e-puck</sub>'''. Again, this value differs from e-puck to e-puck, and also remains constant for each e-puck regardless of orientation.<br />
<br />
The actual code which shifts the coordinate data from '''R<sub>dots</sub>''' to the desired '''R<sub>e-puck</sub>''' is simple.<br />
<br />
In '''main.cpp''' of '''RGBVisionTracking.proj''', the code is as follows:<br />
<br />
line '''1082''' through line '''1109'''<br />
targets_temp[camerai]->group.wx = targets_temp[camerai]->group.wx + -1*targets_temp[camerai]->centermag*cos(Adjusted);<br />
targets_temp[camerai]->group.wy = targets_temp[camerai]->group.wy + -1*targets_temp[camerai]->centermag*sin(Adjusted);<br />
<br />
'''Adjusted''' is the center offset angle plus the orientation angle, or '''centeroffset + angle''', which results is the angle of the line from '''R<sub>dots</sub>''' to '''R<sub>e-puck</sub>''', from zero degrees. With knowledge of this angle, a simply trigonometry problem is performed to determine the x and y values to add/subtract from '''R<sub>dots</sub>''' to get coordinates for '''R<sub>e-puck</sub>'''.<br />
<br />
The values for centermag and centeroffset are found in the txt file, '''target_classifiers_augmented''', as the fourth and fifth values, respectively. This file can be round in the modified code zip file above.<br />
<br />
===Other Considerations===<br />
While there do not need to be any additional changes to the set up of the machine vision localization system, there may be additional considerations for further development. One such consideration is the 'background', or floor material, of the setup. With the modified machine vision code, light intensity is what is picked up and filtered by the system, thus rendering the LEDs from the e-pucks to be the only tracked objects. However, with more advanced set ups, such as one featuring light that is projected onto the background, this may present a problem with the machine vision system picking up reflected light. More testing has to be done with modifying the machine vision system threshold to see if there is an ideal threshold to accommodate this setup. Another option may be to use a non-reflective or matte surface for the background.<br />
<br />
Another consideration involves the hardware of the setup, or the themselves. The cameras are equipped with the Logitech software which automatically adjusts the exposure and light contrast settings to correct for poor lighting and setup conditions. However, this leads to issues as with increased exposure (due to light blocking set up) and bright LEDs results in blurry or blobby images received. The machine vision localization system cannot read these images, and as a result cannot track the e-pucks. One potential solution may be to adjust the threshold of the vision system. Other solutions may be to use LEDs with a lower millicandela, or increase the background lighting of the setup, for instance with upward casting lights.<br />
<br />
==RGB Swarm E-puck Code Overview==<br />
This is an overview<br />
Detailed code outline: [[Swarm RGB E-puck Code]]<br />
<br />
==MATLAB Code for RGB==<br />
<br />
[[Media:RGB_Swarm_MATLAB_09-03-09.zip]]<br />
<br />
====What to do with the files:====<br />
* '''open_serial.m:''' Change the COM call to whatever COM port your XBee radio is plugged into (COM1, COM2, ...). Run this to open the COM port before you run anything else. It will create a COM object in your workspace. It takes a long time, be patient.<br />
* '''close_serial.m:''' Run after you're done to close the COM port. Keeps matlab and your computer happy.<br />
* '''RGB_Swarm_Data_Grabber.m:''' Use this to plot the values from the RGB sensor of one puck. It shows a moving plot. Handy for testing.<br />
* '''RGB_avg_std_logger.m:''' Interactive point logger. Follow the prompts to grab some nice data. Useful for testing.<br />
* '''color_hist.m:''' Plot a histogram from RGB_Swarm_Data_Grabber.<br />
<br />
==Physical Setup==<br />
In the RGB swarm robot project, the epucks pick up light from a projector. This projector has to project onto the floor so that the top mounted light sensors can pick up the light. The floor which the epucks roll on must be completely enclosed so that the only light which reaches it, is the light from the projector. Also this floor must be smooth, flat and durable. See the overhead view below.<br />
<br />
{| align="left" cellpadding = "25" <br />
! [[Image:RGBswarmsetup.jpg|600px|center]]<br />
|}<br />
<br />
<br clear=all><br />
<br />
===Curtains===<br />
The floor is enclosed by two walls and 6 curtains. Two bars protrude from the walls and are connected by an L-joint. There are 3 Eclipse absolute zero curtains on each bar (see diagram). These curtains block 100% light and are sewn together so that no light comes through between them. Covering the whole enclosure, above the projector mount are 7 more curtains sewn together to block all light. <br />
<br />
<br />
<br />
===Floor===<br />
The floor is currently a vinyl sheet from home depot. The sheet had a raised pattern on the front, so it was flipped over. The underside of the vinyl floor is not very durable, but holds up as long as no shoes are used when stepping on it. In the future, a more durable light colored floor may want to be used. Also, the floor is taped down with packing tape. The tape should not be removed as the floor has markings which show where the camera calibration dots should go and where the projected image should fall.<br />
<br />
<br />
<br />
===Projector===<br />
The projector is a Benq MP771 DLP prpjector. It has a digital user manual on a CD in the projection computer. <br />
<br />
Since it is DLP, it has an array of tiny mirrors which reflect the light from the bulb. The light from the bulb is shown through a color wheel which shines red, green, and blue on to the mirror array. The frequency with which the mirrors turn on and off (reflect light and don't) determines the intensity of light. For example if a dark red was being projected, the mirrors would be on more than off in a certain interval. In the case of our projector that interval is %%??8.2 millisecond??%%.<br />
<br />
Each mirror represents a different pixel projected from the projector. This projector has a resolution of ?1024 x 760?, so in order to get a 1 to 1 pixel ratio, the projection computer should be set to display at 1024 x 760. <br />
<br />
The projector should not be tilted forward or backward more that 15°. Because of this and the wide throw of the projector, a keystone projection shape could not be avoided. The projector is programmed to project at the maximum amount of keystone.<br />
<br />
The size of the projected image....<br />
<br />
<br />
<br />
===Projector Mount===<br />
Etiam a odio sit amet nunc ullamcorper fringilla non id dui. Sed interdum, ligula pretium aliquet ornare, ligula metus sodales nisi, in fermentum erat purus id diam. Suspendisse tincidunt scelerisque enim, vitae hendrerit nunc suscipit ac. Cras luctus, nisl eu congue sagittis, nibh quam ultricies eros, vitae ultricies odio mauris vitae urna. Vestibulum ante ipsum primis in faucibus orci luctus et ultrices posuere cubilia Curae; Nam in felis cursus urna pretium scelerisque. Morbi lacinia semper venenatis. Integer pharetra risus et purus luctus pellentesque. Nam risus justo, consectetur nec vestibulum eu, molestie eget felis. Sed tristique congue nulla eget luctus. In suscipit dignissim mollis. Morbi ac pellentesque lacus. Sed vel mauris at nulla imperdiet hendrerit id et sem. Sed elementum semper quam sed feugiat. Suspendisse quis felis vitae sapien placerat aliquet nec quis mauris. Vivamus eget auctor diam. Suspendisse convallis leo nec turpis iaculis quis fringilla libero hendrerit.<br />
<br />
=='''Conclusion'''==<br />
<br />
The new XBee Interface Extension Board design was tested, and we found out that it does not have any problem. In addition, the black dot pattern of the e-pucks are upgraded to LED patterns. The advantage of this improvement is that the machine vision system can recoginize each e-puck no matter where the e-pucks are located. The color of the background also does not affect the vision system. However, we had to move the color sensor to the LED pattern board since the LED pattern board will block the sensor if the sensor is located in the XBee Interface Extension Board. Thus, we now consider the light interference between the LEDs and the color sensor. In the light interference test, we found out that the color sensor is affected by the light from LED. However, since we used much brighter LEDs in our light interference test than the LEDs used for the LED pattern board, we have to do more experiment on this in order to have more accurate interference data. <br />
<br />
<br />
[[Category:SwarmRobotProject]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_E-puck_Code_(outdated)&diff=14265RGB Swarm Robot Project E-puck Code (outdated)2009-09-04T17:16:42Z<p>Sam Bobb: /* Packet Length Constants */</p>
<hr />
<div>This page documents the e-puck code for the RGB Sensing Swarm Robotics project. The code on the e-puck was written in C and compiled using Microchip's MPLAB C Compiler for dsPIC DSCs (student version). <br />
<br />
This code is a branch of the [[Swarm Project E-puck Code]].<br />
<br />
==Tasks==<br />
<br />
===Complete===<br />
* Restructured code to make more modular.<br />
** Split dsPIC_XBeePackets and wheel_speed_coordinator into h and c files<br />
** Pulled packet assembling code out of main and created send_packet() function in send_packet.h/c.<br />
** Pulled a bunch of variables and defines (''NUM_DATA_SETS, NUMBERS_PER_SET, DATATYPE_BYTELENGTH , DATA_ARRAY_LENGTH , ADDITIONAL_NUMS, notRTS, T1_INT_FLAG, x_i, u_i, w_i, x_sum, w_sum, MAX_WHEEL_V_TICKS, deadband, COMMR, SAFEDIST, MINDIST, u_x_ideal, u_y_ideal, x_motion_integral, y_motion_integral, SQUARE'') that were scattered across h files into global_vars.h/c. Makes it easy to include them in a particular file with the ''extern'' keyword.<br />
* Added color_cal() function in color_cal.h/c<br />
* Added wheelSpeedSingleBot to wheel_speed_coordinator<br />
<br />
===To Do===<br />
* Finish color_cal<br />
* Make the vision system position information updater smarter.<br />
* Replace wheelSpeedSingleBot with the three step move controller from NUtest.c<br />
* Implement new algorithm<br />
<br />
<br />
==Project Package==<br />
The source code for the project is available here: [[Media:RGB_Swarm_Puck_Code_working_version.zip]]. Open swarm_epucks.mcw and you should be good to go.<br />
<br />
==Description of the files and functions==<br />
<br />
===global_vars===<br />
* .c/.h: declare and define global variables and macros<br />
====Packet Length Constants====<br />
These variables determine the length of the XBee packets. See [[Swarm_Robot_Project_Documentation#Data_Frame|Data Frame]] and the section on XBee API packets in the XBee manual for further clarification.<br />
<br />
Much of this is still sending integral consensus estimator data. This can be removed or replaces with data needed for decentralized color sensing. <br />
<br />
=====#define NUM_DATA_SETS 5 ===== <br />
Number of statistics on which you are running the consensus estimator. This this particular case, 5. (Ix, Iy, Ixx, Ixy, Iyy)<br />
<br />
=====#define NUMBERS_PER_SET 2=====<br />
Number of variables in each data set (see above) that the consensus estimator needs to transmit to other agents. In this case, 2 because there is <tt>x_i</tt> and <tt>w_i</tt> for each statistic.<br />
<br />
=====#define DATATYPE_BYTELENGTH 4 =====<br />
Number of bytes in the data type (used in the consensus estimator (float = 4 bytes long). This is important because we need to split the numbers into individual bytes to be able to send them out the serial port.<br />
<br />
=====#define DATA_ARRAY_LENGTH (NUM_DATA_SETS*NUMBERS_PER_SET)=====<br />
Total number of data variables needed for the consensus estimator. In this case, it is 5*2=10.<br />
<br />
=====#define ADDITIONAL_NUMS 8=====<br />
Additional number of data to be appended to data array. It is 5 in this case, so that we can append <br />
# Robot X coordinate<br />
# Robot Y coordinate<br />
# Robot Theta orientation<br />
# Robot left wheel speed<br />
# Robot right wheel speed<br />
# Sensor Red Value<br />
# Sensor Green Value<br />
# Sensor Blue Value<br />
<br />
===main===<br />
* .c: This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines. <br />
* .h: Contains variables, function prototypes, and delay function needed for main.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void)=====<br />
Timer1 ISR. Sets T1_INT_FLAG which provides timing for the main loop.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)=====<br />
UART2 receive ISR. Runs with the XBee receives packets. The switch/case structure handles the packets.<br />
<br />
The current handling of coordinate data from the vision system: <br />
<br />
<code><pre><br />
case 0: //coordinate data<br />
e_set_configuration(<br />
InPacket.data[1].dataFloat,<br />
InPacket.data[2].dataFloat, <br />
InPacket.data[3].dataFloat);<br />
break;<br />
</pre></code><br />
<br />
Needs to be improved. Suggested new function:<br />
<br />
* Get vision system data<br />
* Check if it's wildly off from current puck estimate<br />
** If not, update, clear log of rejected packets<br />
** If so, reject and log<br />
*** If we have rejected enough packets (some threshold) and they've all been in similar place (within tolerance), we can assume the puck is wrong and vision system is right. In this case, update with vision system data and clear rejected data log.<br />
<br />
=====int main(void)=====<br />
Setup functions and main loop.<br />
<br />
===color_cal===<br />
* .c: Contains void calibrate_color(void) function to run the calibration routine.<br />
* .h: Contains function prototype and constant definitions for calibrate_color.<br />
<br />
=====void calibrate_color(void)=====<br />
This function runs the color calibration routine. Eventually this should be turned into a separate e-puck command from the vision system. Currently it just runs the puck through a zig zag pattern on the floor and sends packets. You can collect these with the data logger and process them in matlab. From this, you should be able to get a calibration function.<br />
<br />
'''To do:'''<br />
* Add to the pattern. Need to collect more data.<br />
* Possibly have the puck store data, find a best fit, and create the calibration function on board.<br />
* Store the calibration function in the EEPROM (flash memory) so it's non-volatile. The coefficients of function will be specific to each puck, so it would be nice to not have to program each puck, each time you change the battery.<br />
<br />
===dsPIC_XBeePackets===<br />
* .c/.h: Contains functions and data structures for assembling and receiving XBee packets.<br />
<br />
=====int readPacket( void)=====<br />
Parses XBee data from UART2. Makes it accessable in the ''InPacket'' struct.<br />
<br />
The UART2 receive ISR takes error codes from this function and flashes the corresponding puck LED (absolute value of the error code).<br />
<br />
Error codes are:<br />
<br />
<code><pre><br />
//////////////ERROR CODES/////////////////<br />
// <br />
// -1: Timeout waiting for UART2<br />
// -2: Start delimiter wrong<br />
// -3: Checksum Error<br />
// -4: UART2 not ready at beginning<br />
// -5: API_ID unidentified<br />
// -6: Send Packet CCA failuue<br />
// -7: Modem Status packet failure<br />
//////////////////////////////////////////<br />
</pre></code><br />
<br />
===send_packet===<br />
* .c: Contains the void send_packet(void) function which fills an array with data and calls the needed XBee functions to send a packet.<br />
* .h: Contains function prototype.<br />
<br />
=====void send_packet(void)=====<br />
* Creates ''packet'' array.<br />
* Adds consensus estimator data to the array.<br />
* Adds robot statistics to the array.<br />
* Adds color sensor values to the array.<br />
* Asserts flow control line to stop XBee from sending<br />
* Calls assemblePacket to send the packet.<br />
* Deasserts flow control; Xbee can send again.<br />
<br />
<br />
The function e_get_acc_filtered returns a running average of the acc specified. Syntax is:<br />
<br />
return value = e_get_acc_filtered(acc_channel, number of samples to average)<br />
<br />
The number of samples averaged must be less than ACC_SAMP_NB as defined in e_ad_conv.h.<br />
<br />
<code><pre><br />
packet[i]=e_get_acc_filtered(2, 136); // red<br />
i++;<br />
packet[i]=e_get_acc_filtered(1, 136); // green<br />
i++;<br />
packet[i]=e_get_acc_filtered(0, 136); // blue<br />
</pre></code><br />
<br />
===PI_consensus_estimator===<br />
* .h: Contains functions and data structures for the PI consensus estimator. <br />
* This will probably be replaced by the algorithm for sensor consensus. <br />
<br />
===wheel_speed_coordinator===<br />
* .c: Contains functions for robot motion control<br />
* .h: Function prototypes and variabls.<br />
<br />
=====void wheelSpeed(int *vL, int *vR)=====<br />
Return needed wheel speeds for the inertial consensus estimator based on the group goal.<br />
<br />
=====void wheelSpeedSingleBot(float gotox, float gotoy, int *vL, int *vR)=====<br />
Return needed wheel speed to get this individual bot to (gotox, gotoy). It's a hacked fix. Should be replaced with the 3 step motion controller from NUtest.c.<br />
<br />
===e_acc===<br />
* .c: Functions for reading the accelerometers (which is the color sensor).<br />
* .h: Function prototypes.<br />
<br />
This is original e-puck library code with the following modifications:<br />
<br />
<code><pre><br />
//changed by Sam, July 10, default offset is 2000. we want 0 for RGB sensor.<br />
static int centre_z = 0; //zero value for z axe<br />
</pre></code><br />
<br />
=====int e_get_acc_filtered(unsigned int captor, unsigned int filter_size)=====<br />
<br />
===e_ad_conv===<br />
Set up the ADCs on the puck. Original e-puck library code, with the following modifications<br />
* .h: Define constants and functional prototypes<br />
<br />
MIC_SAMP_FREQ sets the baseline sampling frequency for the ADC, everything else must be a fraction of this. 16384 Hz is the highest possible.<br />
<code><pre><br />
#define MIC_SAMP_FREQ 16384.0 <br />
</pre></code><br />
<br />
ACC_PROX_SAMP_FREQ sets the sampling frequency for the accelerometers (color sensor). We found in testing that the puck become non-responsive with this set to 8192 Hz or 16384 Hz.<br />
<code><pre><br />
// sampling frequency for the accelerometres and proximetres<br />
//#define ACC_PROX_SAMP_FREQ 256.0 // WARNING: should be a fraction of MIC_SAMP_FREQ<br />
#define ACC_PROX_SAMP_FREQ 4096 // to ensure a good timing precision<br />
// So your options are: 1 2 4 8 16 32 64 128 <br />
// 256 512 1024 2048 4096 8192 16384<br />
</pre></code><br />
<br />
ACC_SAMP_NB is the number of samples to store. We can do an average of ''up to'' this many samples. This is set to 140 so we can average 136 samples, which is 4 projector periods.<br />
<code><pre><br />
#define ACC_SAMP_NB 140 // number of accelerometer samples to store<br />
</pre></code><br />
<br />
* .c: Functions and interrupt service routines for ADCs. Original e-puck library, no modifications.<br />
<br />
=====e_init_ad_scan(ALL_ADC)=====<br />
Call to setup ADC and have it work in the background. Use e_acc functions to access data.<br />
<br />
===e_init_port===<br />
* .c/.h: Initializes the ports on the e-puck. File is from the standard e-puck library. <br />
<br />
=====e_init_ports(void)=====<br />
This function sets up ports on the e-puck.<br />
<br />
===e_led===<br />
* .c/.h: This is a standard e-puck library file that contains functions for manipulating LEDs.<br />
<br />
=====void e_set_led(unsigned int led_number, unsigned int value)=====<br />
Set led_number (0-7) to value (0=off 1=on higher=inverse).<br />
<br />
[[Image:e-puck_LED_numbering.png|thumb|left]]<br />
<br />
<br clear='all'><br />
<br />
===e_motors_swarm===<br />
* .c/.h: This file is a modified version of the e_motors.h e-puck library file. This version keeps track of the robot's position and orientation, and the motor stepping function contains code to update the robot's position when the wheels turn. <br />
* The functions like e_rotate and e_translate have been removed and this version is not dependent on e_agenda. <br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T5Interrupt(void)=====<br />
Timer5 ISR, interrupt for left motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos + cval*LINSTEP;<br />
ypos = ypos + sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T4Interrupt(void)=====<br />
Timer4 ISR, interrupt for right motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos - cval*LINSTEP;<br />
ypos = ypos - sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void e_init_motors(void)=====<br />
Call this function before other motor functions to initialize the motors.<br />
<br />
=====void e_set_speed_left(int motor_speed)/void e_set_speed_right(int motor_speed)=====<br />
Set the motor speed in steps/second.<br />
<br />
=====void e_get_configuration(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the center reference point.<br />
<br />
=====void e_set_configuration(float x, float y, float theta)=====<br />
Sets x, y, theta to values.<br />
<br />
=====void e_get_configuration_front(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the front reference point (used for motor control).<br />
<br />
<br />
[[Category:SwarmRobotProject]]<br />
[[Category:e-puck]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_E-puck_Code_(outdated)&diff=14264RGB Swarm Robot Project E-puck Code (outdated)2009-09-04T17:16:10Z<p>Sam Bobb: /* define NUM_DATA_SETS 5 */</p>
<hr />
<div>This page documents the e-puck code for the RGB Sensing Swarm Robotics project. The code on the e-puck was written in C and compiled using Microchip's MPLAB C Compiler for dsPIC DSCs (student version). <br />
<br />
This code is a branch of the [[Swarm Project E-puck Code]].<br />
<br />
==Tasks==<br />
<br />
===Complete===<br />
* Restructured code to make more modular.<br />
** Split dsPIC_XBeePackets and wheel_speed_coordinator into h and c files<br />
** Pulled packet assembling code out of main and created send_packet() function in send_packet.h/c.<br />
** Pulled a bunch of variables and defines (''NUM_DATA_SETS, NUMBERS_PER_SET, DATATYPE_BYTELENGTH , DATA_ARRAY_LENGTH , ADDITIONAL_NUMS, notRTS, T1_INT_FLAG, x_i, u_i, w_i, x_sum, w_sum, MAX_WHEEL_V_TICKS, deadband, COMMR, SAFEDIST, MINDIST, u_x_ideal, u_y_ideal, x_motion_integral, y_motion_integral, SQUARE'') that were scattered across h files into global_vars.h/c. Makes it easy to include them in a particular file with the ''extern'' keyword.<br />
* Added color_cal() function in color_cal.h/c<br />
* Added wheelSpeedSingleBot to wheel_speed_coordinator<br />
<br />
===To Do===<br />
* Finish color_cal<br />
* Make the vision system position information updater smarter.<br />
* Replace wheelSpeedSingleBot with the three step move controller from NUtest.c<br />
* Implement new algorithm<br />
<br />
<br />
==Project Package==<br />
The source code for the project is available here: [[Media:RGB_Swarm_Puck_Code_working_version.zip]]. Open swarm_epucks.mcw and you should be good to go.<br />
<br />
==Description of the files and functions==<br />
<br />
===global_vars===<br />
* .c/.h: declare and define global variables and macros<br />
====Packet Length Constants====<br />
These variables determine the length of the XBee packets. See [[Swarm_Robot_Project_Documentation#Data_Frame|Data Frame]] and the section on XBee API packets in the XBee manual for further clarification.<br />
<br />
Much of this is still sending integral consensus estimator data. This can be removed or replaces with data needed for decentralized color sensing. <br />
<br />
======#define NUM_DATA_SETS 5 ====== <br />
Number of statistics on which you are running the consensus estimator. This this particular case, 5. (Ix, Iy, Ixx, Ixy, Iyy)<br />
<br />
======#define NUMBERS_PER_SET 2======<br />
Number of variables in each data set (see above) that the consensus estimator needs to transmit to other agents. In this case, 2 because there is <tt>x_i</tt> and <tt>w_i</tt> for each statistic.<br />
<br />
======#define DATATYPE_BYTELENGTH 4 ======<br />
Number of bytes in the data type (used in the consensus estimator (float = 4 bytes long). This is important because we need to split the numbers into individual bytes to be able to send them out the serial port.<br />
<br />
======#define DATA_ARRAY_LENGTH (NUM_DATA_SETS*NUMBERS_PER_SET)======<br />
Total number of data variables needed for the consensus estimator. In this case, it is 5*2=10.<br />
<br />
======#define ADDITIONAL_NUMS 8======<br />
Additional number of data to be appended to data array. It is 5 in this case, so that we can append <br />
# Robot X coordinate<br />
# Robot Y coordinate<br />
# Robot Theta orientation<br />
# Robot left wheel speed<br />
# Robot right wheel speed<br />
# Sensor Red Value<br />
# Sensor Green Value<br />
# Sensor Blue Value<br />
<br />
===main===<br />
* .c: This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines. <br />
* .h: Contains variables, function prototypes, and delay function needed for main.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void)=====<br />
Timer1 ISR. Sets T1_INT_FLAG which provides timing for the main loop.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)=====<br />
UART2 receive ISR. Runs with the XBee receives packets. The switch/case structure handles the packets.<br />
<br />
The current handling of coordinate data from the vision system: <br />
<br />
<code><pre><br />
case 0: //coordinate data<br />
e_set_configuration(<br />
InPacket.data[1].dataFloat,<br />
InPacket.data[2].dataFloat, <br />
InPacket.data[3].dataFloat);<br />
break;<br />
</pre></code><br />
<br />
Needs to be improved. Suggested new function:<br />
<br />
* Get vision system data<br />
* Check if it's wildly off from current puck estimate<br />
** If not, update, clear log of rejected packets<br />
** If so, reject and log<br />
*** If we have rejected enough packets (some threshold) and they've all been in similar place (within tolerance), we can assume the puck is wrong and vision system is right. In this case, update with vision system data and clear rejected data log.<br />
<br />
=====int main(void)=====<br />
Setup functions and main loop.<br />
<br />
===color_cal===<br />
* .c: Contains void calibrate_color(void) function to run the calibration routine.<br />
* .h: Contains function prototype and constant definitions for calibrate_color.<br />
<br />
=====void calibrate_color(void)=====<br />
This function runs the color calibration routine. Eventually this should be turned into a separate e-puck command from the vision system. Currently it just runs the puck through a zig zag pattern on the floor and sends packets. You can collect these with the data logger and process them in matlab. From this, you should be able to get a calibration function.<br />
<br />
'''To do:'''<br />
* Add to the pattern. Need to collect more data.<br />
* Possibly have the puck store data, find a best fit, and create the calibration function on board.<br />
* Store the calibration function in the EEPROM (flash memory) so it's non-volatile. The coefficients of function will be specific to each puck, so it would be nice to not have to program each puck, each time you change the battery.<br />
<br />
===dsPIC_XBeePackets===<br />
* .c/.h: Contains functions and data structures for assembling and receiving XBee packets.<br />
<br />
=====int readPacket( void)=====<br />
Parses XBee data from UART2. Makes it accessable in the ''InPacket'' struct.<br />
<br />
The UART2 receive ISR takes error codes from this function and flashes the corresponding puck LED (absolute value of the error code).<br />
<br />
Error codes are:<br />
<br />
<code><pre><br />
//////////////ERROR CODES/////////////////<br />
// <br />
// -1: Timeout waiting for UART2<br />
// -2: Start delimiter wrong<br />
// -3: Checksum Error<br />
// -4: UART2 not ready at beginning<br />
// -5: API_ID unidentified<br />
// -6: Send Packet CCA failuue<br />
// -7: Modem Status packet failure<br />
//////////////////////////////////////////<br />
</pre></code><br />
<br />
===send_packet===<br />
* .c: Contains the void send_packet(void) function which fills an array with data and calls the needed XBee functions to send a packet.<br />
* .h: Contains function prototype.<br />
<br />
=====void send_packet(void)=====<br />
* Creates ''packet'' array.<br />
* Adds consensus estimator data to the array.<br />
* Adds robot statistics to the array.<br />
* Adds color sensor values to the array.<br />
* Asserts flow control line to stop XBee from sending<br />
* Calls assemblePacket to send the packet.<br />
* Deasserts flow control; Xbee can send again.<br />
<br />
<br />
The function e_get_acc_filtered returns a running average of the acc specified. Syntax is:<br />
<br />
return value = e_get_acc_filtered(acc_channel, number of samples to average)<br />
<br />
The number of samples averaged must be less than ACC_SAMP_NB as defined in e_ad_conv.h.<br />
<br />
<code><pre><br />
packet[i]=e_get_acc_filtered(2, 136); // red<br />
i++;<br />
packet[i]=e_get_acc_filtered(1, 136); // green<br />
i++;<br />
packet[i]=e_get_acc_filtered(0, 136); // blue<br />
</pre></code><br />
<br />
===PI_consensus_estimator===<br />
* .h: Contains functions and data structures for the PI consensus estimator. <br />
* This will probably be replaced by the algorithm for sensor consensus. <br />
<br />
===wheel_speed_coordinator===<br />
* .c: Contains functions for robot motion control<br />
* .h: Function prototypes and variabls.<br />
<br />
=====void wheelSpeed(int *vL, int *vR)=====<br />
Return needed wheel speeds for the inertial consensus estimator based on the group goal.<br />
<br />
=====void wheelSpeedSingleBot(float gotox, float gotoy, int *vL, int *vR)=====<br />
Return needed wheel speed to get this individual bot to (gotox, gotoy). It's a hacked fix. Should be replaced with the 3 step motion controller from NUtest.c.<br />
<br />
===e_acc===<br />
* .c: Functions for reading the accelerometers (which is the color sensor).<br />
* .h: Function prototypes.<br />
<br />
This is original e-puck library code with the following modifications:<br />
<br />
<code><pre><br />
//changed by Sam, July 10, default offset is 2000. we want 0 for RGB sensor.<br />
static int centre_z = 0; //zero value for z axe<br />
</pre></code><br />
<br />
=====int e_get_acc_filtered(unsigned int captor, unsigned int filter_size)=====<br />
<br />
===e_ad_conv===<br />
Set up the ADCs on the puck. Original e-puck library code, with the following modifications<br />
* .h: Define constants and functional prototypes<br />
<br />
MIC_SAMP_FREQ sets the baseline sampling frequency for the ADC, everything else must be a fraction of this. 16384 Hz is the highest possible.<br />
<code><pre><br />
#define MIC_SAMP_FREQ 16384.0 <br />
</pre></code><br />
<br />
ACC_PROX_SAMP_FREQ sets the sampling frequency for the accelerometers (color sensor). We found in testing that the puck become non-responsive with this set to 8192 Hz or 16384 Hz.<br />
<code><pre><br />
// sampling frequency for the accelerometres and proximetres<br />
//#define ACC_PROX_SAMP_FREQ 256.0 // WARNING: should be a fraction of MIC_SAMP_FREQ<br />
#define ACC_PROX_SAMP_FREQ 4096 // to ensure a good timing precision<br />
// So your options are: 1 2 4 8 16 32 64 128 <br />
// 256 512 1024 2048 4096 8192 16384<br />
</pre></code><br />
<br />
ACC_SAMP_NB is the number of samples to store. We can do an average of ''up to'' this many samples. This is set to 140 so we can average 136 samples, which is 4 projector periods.<br />
<code><pre><br />
#define ACC_SAMP_NB 140 // number of accelerometer samples to store<br />
</pre></code><br />
<br />
* .c: Functions and interrupt service routines for ADCs. Original e-puck library, no modifications.<br />
<br />
=====e_init_ad_scan(ALL_ADC)=====<br />
Call to setup ADC and have it work in the background. Use e_acc functions to access data.<br />
<br />
===e_init_port===<br />
* .c/.h: Initializes the ports on the e-puck. File is from the standard e-puck library. <br />
<br />
=====e_init_ports(void)=====<br />
This function sets up ports on the e-puck.<br />
<br />
===e_led===<br />
* .c/.h: This is a standard e-puck library file that contains functions for manipulating LEDs.<br />
<br />
=====void e_set_led(unsigned int led_number, unsigned int value)=====<br />
Set led_number (0-7) to value (0=off 1=on higher=inverse).<br />
<br />
[[Image:e-puck_LED_numbering.png|thumb|left]]<br />
<br />
<br clear='all'><br />
<br />
===e_motors_swarm===<br />
* .c/.h: This file is a modified version of the e_motors.h e-puck library file. This version keeps track of the robot's position and orientation, and the motor stepping function contains code to update the robot's position when the wheels turn. <br />
* The functions like e_rotate and e_translate have been removed and this version is not dependent on e_agenda. <br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T5Interrupt(void)=====<br />
Timer5 ISR, interrupt for left motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos + cval*LINSTEP;<br />
ypos = ypos + sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T4Interrupt(void)=====<br />
Timer4 ISR, interrupt for right motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos - cval*LINSTEP;<br />
ypos = ypos - sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void e_init_motors(void)=====<br />
Call this function before other motor functions to initialize the motors.<br />
<br />
=====void e_set_speed_left(int motor_speed)/void e_set_speed_right(int motor_speed)=====<br />
Set the motor speed in steps/second.<br />
<br />
=====void e_get_configuration(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the center reference point.<br />
<br />
=====void e_set_configuration(float x, float y, float theta)=====<br />
Sets x, y, theta to values.<br />
<br />
=====void e_get_configuration_front(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the front reference point (used for motor control).<br />
<br />
<br />
[[Category:SwarmRobotProject]]<br />
[[Category:e-puck]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_E-puck_Code_(outdated)&diff=14263RGB Swarm Robot Project E-puck Code (outdated)2009-09-04T17:15:53Z<p>Sam Bobb: /* #define NUM_DATA_SETS 5 */</p>
<hr />
<div>This page documents the e-puck code for the RGB Sensing Swarm Robotics project. The code on the e-puck was written in C and compiled using Microchip's MPLAB C Compiler for dsPIC DSCs (student version). <br />
<br />
This code is a branch of the [[Swarm Project E-puck Code]].<br />
<br />
==Tasks==<br />
<br />
===Complete===<br />
* Restructured code to make more modular.<br />
** Split dsPIC_XBeePackets and wheel_speed_coordinator into h and c files<br />
** Pulled packet assembling code out of main and created send_packet() function in send_packet.h/c.<br />
** Pulled a bunch of variables and defines (''NUM_DATA_SETS, NUMBERS_PER_SET, DATATYPE_BYTELENGTH , DATA_ARRAY_LENGTH , ADDITIONAL_NUMS, notRTS, T1_INT_FLAG, x_i, u_i, w_i, x_sum, w_sum, MAX_WHEEL_V_TICKS, deadband, COMMR, SAFEDIST, MINDIST, u_x_ideal, u_y_ideal, x_motion_integral, y_motion_integral, SQUARE'') that were scattered across h files into global_vars.h/c. Makes it easy to include them in a particular file with the ''extern'' keyword.<br />
* Added color_cal() function in color_cal.h/c<br />
* Added wheelSpeedSingleBot to wheel_speed_coordinator<br />
<br />
===To Do===<br />
* Finish color_cal<br />
* Make the vision system position information updater smarter.<br />
* Replace wheelSpeedSingleBot with the three step move controller from NUtest.c<br />
* Implement new algorithm<br />
<br />
<br />
==Project Package==<br />
The source code for the project is available here: [[Media:RGB_Swarm_Puck_Code_working_version.zip]]. Open swarm_epucks.mcw and you should be good to go.<br />
<br />
==Description of the files and functions==<br />
<br />
===global_vars===<br />
* .c/.h: declare and define global variables and macros<br />
====Packet Length Constants====<br />
These variables determine the length of the XBee packets. See [[Swarm_Robot_Project_Documentation#Data_Frame|Data Frame]] and the section on XBee API packets in the XBee manual for further clarification.<br />
<br />
Much of this is still sending integral consensus estimator data. This can be removed or replaces with data needed for decentralized color sensing. <br />
<br />
======define NUM_DATA_SETS 5 ====== <br />
Number of statistics on which you are running the consensus estimator. This this particular case, 5. (Ix, Iy, Ixx, Ixy, Iyy)<br />
<br />
======#define NUMBERS_PER_SET 2======<br />
Number of variables in each data set (see above) that the consensus estimator needs to transmit to other agents. In this case, 2 because there is <tt>x_i</tt> and <tt>w_i</tt> for each statistic.<br />
<br />
======#define DATATYPE_BYTELENGTH 4 ======<br />
Number of bytes in the data type (used in the consensus estimator (float = 4 bytes long). This is important because we need to split the numbers into individual bytes to be able to send them out the serial port.<br />
<br />
======#define DATA_ARRAY_LENGTH (NUM_DATA_SETS*NUMBERS_PER_SET)======<br />
Total number of data variables needed for the consensus estimator. In this case, it is 5*2=10.<br />
<br />
======#define ADDITIONAL_NUMS 8======<br />
Additional number of data to be appended to data array. It is 5 in this case, so that we can append <br />
# Robot X coordinate<br />
# Robot Y coordinate<br />
# Robot Theta orientation<br />
# Robot left wheel speed<br />
# Robot right wheel speed<br />
# Sensor Red Value<br />
# Sensor Green Value<br />
# Sensor Blue Value<br />
<br />
===main===<br />
* .c: This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines. <br />
* .h: Contains variables, function prototypes, and delay function needed for main.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void)=====<br />
Timer1 ISR. Sets T1_INT_FLAG which provides timing for the main loop.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)=====<br />
UART2 receive ISR. Runs with the XBee receives packets. The switch/case structure handles the packets.<br />
<br />
The current handling of coordinate data from the vision system: <br />
<br />
<code><pre><br />
case 0: //coordinate data<br />
e_set_configuration(<br />
InPacket.data[1].dataFloat,<br />
InPacket.data[2].dataFloat, <br />
InPacket.data[3].dataFloat);<br />
break;<br />
</pre></code><br />
<br />
Needs to be improved. Suggested new function:<br />
<br />
* Get vision system data<br />
* Check if it's wildly off from current puck estimate<br />
** If not, update, clear log of rejected packets<br />
** If so, reject and log<br />
*** If we have rejected enough packets (some threshold) and they've all been in similar place (within tolerance), we can assume the puck is wrong and vision system is right. In this case, update with vision system data and clear rejected data log.<br />
<br />
=====int main(void)=====<br />
Setup functions and main loop.<br />
<br />
===color_cal===<br />
* .c: Contains void calibrate_color(void) function to run the calibration routine.<br />
* .h: Contains function prototype and constant definitions for calibrate_color.<br />
<br />
=====void calibrate_color(void)=====<br />
This function runs the color calibration routine. Eventually this should be turned into a separate e-puck command from the vision system. Currently it just runs the puck through a zig zag pattern on the floor and sends packets. You can collect these with the data logger and process them in matlab. From this, you should be able to get a calibration function.<br />
<br />
'''To do:'''<br />
* Add to the pattern. Need to collect more data.<br />
* Possibly have the puck store data, find a best fit, and create the calibration function on board.<br />
* Store the calibration function in the EEPROM (flash memory) so it's non-volatile. The coefficients of function will be specific to each puck, so it would be nice to not have to program each puck, each time you change the battery.<br />
<br />
===dsPIC_XBeePackets===<br />
* .c/.h: Contains functions and data structures for assembling and receiving XBee packets.<br />
<br />
=====int readPacket( void)=====<br />
Parses XBee data from UART2. Makes it accessable in the ''InPacket'' struct.<br />
<br />
The UART2 receive ISR takes error codes from this function and flashes the corresponding puck LED (absolute value of the error code).<br />
<br />
Error codes are:<br />
<br />
<code><pre><br />
//////////////ERROR CODES/////////////////<br />
// <br />
// -1: Timeout waiting for UART2<br />
// -2: Start delimiter wrong<br />
// -3: Checksum Error<br />
// -4: UART2 not ready at beginning<br />
// -5: API_ID unidentified<br />
// -6: Send Packet CCA failuue<br />
// -7: Modem Status packet failure<br />
//////////////////////////////////////////<br />
</pre></code><br />
<br />
===send_packet===<br />
* .c: Contains the void send_packet(void) function which fills an array with data and calls the needed XBee functions to send a packet.<br />
* .h: Contains function prototype.<br />
<br />
=====void send_packet(void)=====<br />
* Creates ''packet'' array.<br />
* Adds consensus estimator data to the array.<br />
* Adds robot statistics to the array.<br />
* Adds color sensor values to the array.<br />
* Asserts flow control line to stop XBee from sending<br />
* Calls assemblePacket to send the packet.<br />
* Deasserts flow control; Xbee can send again.<br />
<br />
<br />
The function e_get_acc_filtered returns a running average of the acc specified. Syntax is:<br />
<br />
return value = e_get_acc_filtered(acc_channel, number of samples to average)<br />
<br />
The number of samples averaged must be less than ACC_SAMP_NB as defined in e_ad_conv.h.<br />
<br />
<code><pre><br />
packet[i]=e_get_acc_filtered(2, 136); // red<br />
i++;<br />
packet[i]=e_get_acc_filtered(1, 136); // green<br />
i++;<br />
packet[i]=e_get_acc_filtered(0, 136); // blue<br />
</pre></code><br />
<br />
===PI_consensus_estimator===<br />
* .h: Contains functions and data structures for the PI consensus estimator. <br />
* This will probably be replaced by the algorithm for sensor consensus. <br />
<br />
===wheel_speed_coordinator===<br />
* .c: Contains functions for robot motion control<br />
* .h: Function prototypes and variabls.<br />
<br />
=====void wheelSpeed(int *vL, int *vR)=====<br />
Return needed wheel speeds for the inertial consensus estimator based on the group goal.<br />
<br />
=====void wheelSpeedSingleBot(float gotox, float gotoy, int *vL, int *vR)=====<br />
Return needed wheel speed to get this individual bot to (gotox, gotoy). It's a hacked fix. Should be replaced with the 3 step motion controller from NUtest.c.<br />
<br />
===e_acc===<br />
* .c: Functions for reading the accelerometers (which is the color sensor).<br />
* .h: Function prototypes.<br />
<br />
This is original e-puck library code with the following modifications:<br />
<br />
<code><pre><br />
//changed by Sam, July 10, default offset is 2000. we want 0 for RGB sensor.<br />
static int centre_z = 0; //zero value for z axe<br />
</pre></code><br />
<br />
=====int e_get_acc_filtered(unsigned int captor, unsigned int filter_size)=====<br />
<br />
===e_ad_conv===<br />
Set up the ADCs on the puck. Original e-puck library code, with the following modifications<br />
* .h: Define constants and functional prototypes<br />
<br />
MIC_SAMP_FREQ sets the baseline sampling frequency for the ADC, everything else must be a fraction of this. 16384 Hz is the highest possible.<br />
<code><pre><br />
#define MIC_SAMP_FREQ 16384.0 <br />
</pre></code><br />
<br />
ACC_PROX_SAMP_FREQ sets the sampling frequency for the accelerometers (color sensor). We found in testing that the puck become non-responsive with this set to 8192 Hz or 16384 Hz.<br />
<code><pre><br />
// sampling frequency for the accelerometres and proximetres<br />
//#define ACC_PROX_SAMP_FREQ 256.0 // WARNING: should be a fraction of MIC_SAMP_FREQ<br />
#define ACC_PROX_SAMP_FREQ 4096 // to ensure a good timing precision<br />
// So your options are: 1 2 4 8 16 32 64 128 <br />
// 256 512 1024 2048 4096 8192 16384<br />
</pre></code><br />
<br />
ACC_SAMP_NB is the number of samples to store. We can do an average of ''up to'' this many samples. This is set to 140 so we can average 136 samples, which is 4 projector periods.<br />
<code><pre><br />
#define ACC_SAMP_NB 140 // number of accelerometer samples to store<br />
</pre></code><br />
<br />
* .c: Functions and interrupt service routines for ADCs. Original e-puck library, no modifications.<br />
<br />
=====e_init_ad_scan(ALL_ADC)=====<br />
Call to setup ADC and have it work in the background. Use e_acc functions to access data.<br />
<br />
===e_init_port===<br />
* .c/.h: Initializes the ports on the e-puck. File is from the standard e-puck library. <br />
<br />
=====e_init_ports(void)=====<br />
This function sets up ports on the e-puck.<br />
<br />
===e_led===<br />
* .c/.h: This is a standard e-puck library file that contains functions for manipulating LEDs.<br />
<br />
=====void e_set_led(unsigned int led_number, unsigned int value)=====<br />
Set led_number (0-7) to value (0=off 1=on higher=inverse).<br />
<br />
[[Image:e-puck_LED_numbering.png|thumb|left]]<br />
<br />
<br clear='all'><br />
<br />
===e_motors_swarm===<br />
* .c/.h: This file is a modified version of the e_motors.h e-puck library file. This version keeps track of the robot's position and orientation, and the motor stepping function contains code to update the robot's position when the wheels turn. <br />
* The functions like e_rotate and e_translate have been removed and this version is not dependent on e_agenda. <br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T5Interrupt(void)=====<br />
Timer5 ISR, interrupt for left motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos + cval*LINSTEP;<br />
ypos = ypos + sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T4Interrupt(void)=====<br />
Timer4 ISR, interrupt for right motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos - cval*LINSTEP;<br />
ypos = ypos - sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void e_init_motors(void)=====<br />
Call this function before other motor functions to initialize the motors.<br />
<br />
=====void e_set_speed_left(int motor_speed)/void e_set_speed_right(int motor_speed)=====<br />
Set the motor speed in steps/second.<br />
<br />
=====void e_get_configuration(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the center reference point.<br />
<br />
=====void e_set_configuration(float x, float y, float theta)=====<br />
Sets x, y, theta to values.<br />
<br />
=====void e_get_configuration_front(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the front reference point (used for motor control).<br />
<br />
<br />
[[Category:SwarmRobotProject]]<br />
[[Category:e-puck]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_E-puck_Code_(outdated)&diff=14262RGB Swarm Robot Project E-puck Code (outdated)2009-09-04T17:15:10Z<p>Sam Bobb: /* dsPIC_XBeePackets */</p>
<hr />
<div>This page documents the e-puck code for the RGB Sensing Swarm Robotics project. The code on the e-puck was written in C and compiled using Microchip's MPLAB C Compiler for dsPIC DSCs (student version). <br />
<br />
This code is a branch of the [[Swarm Project E-puck Code]].<br />
<br />
==Tasks==<br />
<br />
===Complete===<br />
* Restructured code to make more modular.<br />
** Split dsPIC_XBeePackets and wheel_speed_coordinator into h and c files<br />
** Pulled packet assembling code out of main and created send_packet() function in send_packet.h/c.<br />
** Pulled a bunch of variables and defines (''NUM_DATA_SETS, NUMBERS_PER_SET, DATATYPE_BYTELENGTH , DATA_ARRAY_LENGTH , ADDITIONAL_NUMS, notRTS, T1_INT_FLAG, x_i, u_i, w_i, x_sum, w_sum, MAX_WHEEL_V_TICKS, deadband, COMMR, SAFEDIST, MINDIST, u_x_ideal, u_y_ideal, x_motion_integral, y_motion_integral, SQUARE'') that were scattered across h files into global_vars.h/c. Makes it easy to include them in a particular file with the ''extern'' keyword.<br />
* Added color_cal() function in color_cal.h/c<br />
* Added wheelSpeedSingleBot to wheel_speed_coordinator<br />
<br />
===To Do===<br />
* Finish color_cal<br />
* Make the vision system position information updater smarter.<br />
* Replace wheelSpeedSingleBot with the three step move controller from NUtest.c<br />
* Implement new algorithm<br />
<br />
<br />
==Project Package==<br />
The source code for the project is available here: [[Media:RGB_Swarm_Puck_Code_working_version.zip]]. Open swarm_epucks.mcw and you should be good to go.<br />
<br />
==Description of the files and functions==<br />
<br />
===global_vars===<br />
* .c/.h: declare and define global variables and macros<br />
====Packet Length Constants====<br />
These variables determine the length of the XBee packets. See [[Swarm_Robot_Project_Documentation#Data_Frame|Data Frame]] and the section on XBee API packets in the XBee manual for further clarification.<br />
<br />
Much of this is still sending integral consensus estimator data. This can be removed or replaces with data needed for decentralized color sensing. <br />
<br />
======#define NUM_DATA_SETS 5 ====== <br />
Number of statistics on which you are running the consensus estimator. This this particular case, 5. (Ix, Iy, Ixx, Ixy, Iyy)<br />
<br />
======#define NUMBERS_PER_SET 2======<br />
Number of variables in each data set (see above) that the consensus estimator needs to transmit to other agents. In this case, 2 because there is <tt>x_i</tt> and <tt>w_i</tt> for each statistic.<br />
<br />
======#define DATATYPE_BYTELENGTH 4 ======<br />
Number of bytes in the data type (used in the consensus estimator (float = 4 bytes long). This is important because we need to split the numbers into individual bytes to be able to send them out the serial port.<br />
<br />
======#define DATA_ARRAY_LENGTH (NUM_DATA_SETS*NUMBERS_PER_SET)======<br />
Total number of data variables needed for the consensus estimator. In this case, it is 5*2=10.<br />
<br />
======#define ADDITIONAL_NUMS 8======<br />
Additional number of data to be appended to data array. It is 5 in this case, so that we can append <br />
# Robot X coordinate<br />
# Robot Y coordinate<br />
# Robot Theta orientation<br />
# Robot left wheel speed<br />
# Robot right wheel speed<br />
# Sensor Red Value<br />
# Sensor Green Value<br />
# Sensor Blue Value<br />
<br />
===main===<br />
* .c: This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines. <br />
* .h: Contains variables, function prototypes, and delay function needed for main.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void)=====<br />
Timer1 ISR. Sets T1_INT_FLAG which provides timing for the main loop.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)=====<br />
UART2 receive ISR. Runs with the XBee receives packets. The switch/case structure handles the packets.<br />
<br />
The current handling of coordinate data from the vision system: <br />
<br />
<code><pre><br />
case 0: //coordinate data<br />
e_set_configuration(<br />
InPacket.data[1].dataFloat,<br />
InPacket.data[2].dataFloat, <br />
InPacket.data[3].dataFloat);<br />
break;<br />
</pre></code><br />
<br />
Needs to be improved. Suggested new function:<br />
<br />
* Get vision system data<br />
* Check if it's wildly off from current puck estimate<br />
** If not, update, clear log of rejected packets<br />
** If so, reject and log<br />
*** If we have rejected enough packets (some threshold) and they've all been in similar place (within tolerance), we can assume the puck is wrong and vision system is right. In this case, update with vision system data and clear rejected data log.<br />
<br />
=====int main(void)=====<br />
Setup functions and main loop.<br />
<br />
===color_cal===<br />
* .c: Contains void calibrate_color(void) function to run the calibration routine.<br />
* .h: Contains function prototype and constant definitions for calibrate_color.<br />
<br />
=====void calibrate_color(void)=====<br />
This function runs the color calibration routine. Eventually this should be turned into a separate e-puck command from the vision system. Currently it just runs the puck through a zig zag pattern on the floor and sends packets. You can collect these with the data logger and process them in matlab. From this, you should be able to get a calibration function.<br />
<br />
'''To do:'''<br />
* Add to the pattern. Need to collect more data.<br />
* Possibly have the puck store data, find a best fit, and create the calibration function on board.<br />
* Store the calibration function in the EEPROM (flash memory) so it's non-volatile. The coefficients of function will be specific to each puck, so it would be nice to not have to program each puck, each time you change the battery.<br />
<br />
===dsPIC_XBeePackets===<br />
* .c/.h: Contains functions and data structures for assembling and receiving XBee packets.<br />
<br />
=====int readPacket( void)=====<br />
Parses XBee data from UART2. Makes it accessable in the ''InPacket'' struct.<br />
<br />
The UART2 receive ISR takes error codes from this function and flashes the corresponding puck LED (absolute value of the error code).<br />
<br />
Error codes are:<br />
<br />
<code><pre><br />
//////////////ERROR CODES/////////////////<br />
// <br />
// -1: Timeout waiting for UART2<br />
// -2: Start delimiter wrong<br />
// -3: Checksum Error<br />
// -4: UART2 not ready at beginning<br />
// -5: API_ID unidentified<br />
// -6: Send Packet CCA failuue<br />
// -7: Modem Status packet failure<br />
//////////////////////////////////////////<br />
</pre></code><br />
<br />
===send_packet===<br />
* .c: Contains the void send_packet(void) function which fills an array with data and calls the needed XBee functions to send a packet.<br />
* .h: Contains function prototype.<br />
<br />
=====void send_packet(void)=====<br />
* Creates ''packet'' array.<br />
* Adds consensus estimator data to the array.<br />
* Adds robot statistics to the array.<br />
* Adds color sensor values to the array.<br />
* Asserts flow control line to stop XBee from sending<br />
* Calls assemblePacket to send the packet.<br />
* Deasserts flow control; Xbee can send again.<br />
<br />
<br />
The function e_get_acc_filtered returns a running average of the acc specified. Syntax is:<br />
<br />
return value = e_get_acc_filtered(acc_channel, number of samples to average)<br />
<br />
The number of samples averaged must be less than ACC_SAMP_NB as defined in e_ad_conv.h.<br />
<br />
<code><pre><br />
packet[i]=e_get_acc_filtered(2, 136); // red<br />
i++;<br />
packet[i]=e_get_acc_filtered(1, 136); // green<br />
i++;<br />
packet[i]=e_get_acc_filtered(0, 136); // blue<br />
</pre></code><br />
<br />
===PI_consensus_estimator===<br />
* .h: Contains functions and data structures for the PI consensus estimator. <br />
* This will probably be replaced by the algorithm for sensor consensus. <br />
<br />
===wheel_speed_coordinator===<br />
* .c: Contains functions for robot motion control<br />
* .h: Function prototypes and variabls.<br />
<br />
=====void wheelSpeed(int *vL, int *vR)=====<br />
Return needed wheel speeds for the inertial consensus estimator based on the group goal.<br />
<br />
=====void wheelSpeedSingleBot(float gotox, float gotoy, int *vL, int *vR)=====<br />
Return needed wheel speed to get this individual bot to (gotox, gotoy). It's a hacked fix. Should be replaced with the 3 step motion controller from NUtest.c.<br />
<br />
===e_acc===<br />
* .c: Functions for reading the accelerometers (which is the color sensor).<br />
* .h: Function prototypes.<br />
<br />
This is original e-puck library code with the following modifications:<br />
<br />
<code><pre><br />
//changed by Sam, July 10, default offset is 2000. we want 0 for RGB sensor.<br />
static int centre_z = 0; //zero value for z axe<br />
</pre></code><br />
<br />
=====int e_get_acc_filtered(unsigned int captor, unsigned int filter_size)=====<br />
<br />
===e_ad_conv===<br />
Set up the ADCs on the puck. Original e-puck library code, with the following modifications<br />
* .h: Define constants and functional prototypes<br />
<br />
MIC_SAMP_FREQ sets the baseline sampling frequency for the ADC, everything else must be a fraction of this. 16384 Hz is the highest possible.<br />
<code><pre><br />
#define MIC_SAMP_FREQ 16384.0 <br />
</pre></code><br />
<br />
ACC_PROX_SAMP_FREQ sets the sampling frequency for the accelerometers (color sensor). We found in testing that the puck become non-responsive with this set to 8192 Hz or 16384 Hz.<br />
<code><pre><br />
// sampling frequency for the accelerometres and proximetres<br />
//#define ACC_PROX_SAMP_FREQ 256.0 // WARNING: should be a fraction of MIC_SAMP_FREQ<br />
#define ACC_PROX_SAMP_FREQ 4096 // to ensure a good timing precision<br />
// So your options are: 1 2 4 8 16 32 64 128 <br />
// 256 512 1024 2048 4096 8192 16384<br />
</pre></code><br />
<br />
ACC_SAMP_NB is the number of samples to store. We can do an average of ''up to'' this many samples. This is set to 140 so we can average 136 samples, which is 4 projector periods.<br />
<code><pre><br />
#define ACC_SAMP_NB 140 // number of accelerometer samples to store<br />
</pre></code><br />
<br />
* .c: Functions and interrupt service routines for ADCs. Original e-puck library, no modifications.<br />
<br />
=====e_init_ad_scan(ALL_ADC)=====<br />
Call to setup ADC and have it work in the background. Use e_acc functions to access data.<br />
<br />
===e_init_port===<br />
* .c/.h: Initializes the ports on the e-puck. File is from the standard e-puck library. <br />
<br />
=====e_init_ports(void)=====<br />
This function sets up ports on the e-puck.<br />
<br />
===e_led===<br />
* .c/.h: This is a standard e-puck library file that contains functions for manipulating LEDs.<br />
<br />
=====void e_set_led(unsigned int led_number, unsigned int value)=====<br />
Set led_number (0-7) to value (0=off 1=on higher=inverse).<br />
<br />
[[Image:e-puck_LED_numbering.png|thumb|left]]<br />
<br />
<br clear='all'><br />
<br />
===e_motors_swarm===<br />
* .c/.h: This file is a modified version of the e_motors.h e-puck library file. This version keeps track of the robot's position and orientation, and the motor stepping function contains code to update the robot's position when the wheels turn. <br />
* The functions like e_rotate and e_translate have been removed and this version is not dependent on e_agenda. <br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T5Interrupt(void)=====<br />
Timer5 ISR, interrupt for left motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos + cval*LINSTEP;<br />
ypos = ypos + sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T4Interrupt(void)=====<br />
Timer4 ISR, interrupt for right motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos - cval*LINSTEP;<br />
ypos = ypos - sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void e_init_motors(void)=====<br />
Call this function before other motor functions to initialize the motors.<br />
<br />
=====void e_set_speed_left(int motor_speed)/void e_set_speed_right(int motor_speed)=====<br />
Set the motor speed in steps/second.<br />
<br />
=====void e_get_configuration(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the center reference point.<br />
<br />
=====void e_set_configuration(float x, float y, float theta)=====<br />
Sets x, y, theta to values.<br />
<br />
=====void e_get_configuration_front(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the front reference point (used for motor control).<br />
<br />
<br />
[[Category:SwarmRobotProject]]<br />
[[Category:e-puck]]</div>Sam Bobbhttps://hades.mech.northwestern.edu//index.php?title=RGB_Swarm_Robot_Project_E-puck_Code_(outdated)&diff=14260RGB Swarm Robot Project E-puck Code (outdated)2009-09-04T17:08:57Z<p>Sam Bobb: /* void __attribute__((interrupt, auto_psv, shadow)) _T4Interrupt(void) */</p>
<hr />
<div>This page documents the e-puck code for the RGB Sensing Swarm Robotics project. The code on the e-puck was written in C and compiled using Microchip's MPLAB C Compiler for dsPIC DSCs (student version). <br />
<br />
This code is a branch of the [[Swarm Project E-puck Code]].<br />
<br />
==Tasks==<br />
<br />
===Complete===<br />
* Restructured code to make more modular.<br />
** Split dsPIC_XBeePackets and wheel_speed_coordinator into h and c files<br />
** Pulled packet assembling code out of main and created send_packet() function in send_packet.h/c.<br />
** Pulled a bunch of variables and defines (''NUM_DATA_SETS, NUMBERS_PER_SET, DATATYPE_BYTELENGTH , DATA_ARRAY_LENGTH , ADDITIONAL_NUMS, notRTS, T1_INT_FLAG, x_i, u_i, w_i, x_sum, w_sum, MAX_WHEEL_V_TICKS, deadband, COMMR, SAFEDIST, MINDIST, u_x_ideal, u_y_ideal, x_motion_integral, y_motion_integral, SQUARE'') that were scattered across h files into global_vars.h/c. Makes it easy to include them in a particular file with the ''extern'' keyword.<br />
* Added color_cal() function in color_cal.h/c<br />
* Added wheelSpeedSingleBot to wheel_speed_coordinator<br />
<br />
===To Do===<br />
* Finish color_cal<br />
* Make the vision system position information updater smarter.<br />
* Replace wheelSpeedSingleBot with the three step move controller from NUtest.c<br />
* Implement new algorithm<br />
<br />
<br />
==Project Package==<br />
The source code for the project is available here: [[Media:RGB_Swarm_Puck_Code_working_version.zip]]. Open swarm_epucks.mcw and you should be good to go.<br />
<br />
==Description of the files and functions==<br />
<br />
===global_vars===<br />
* .c/.h: declare and define global variables and macros<br />
====Packet Length Constants====<br />
These variables determine the length of the XBee packets. See [[Swarm_Robot_Project_Documentation#Data_Frame|Data Frame]] and the section on XBee API packets in the XBee manual for further clarification.<br />
<br />
Much of this is still sending integral consensus estimator data. This can be removed or replaces with data needed for decentralized color sensing. <br />
<br />
======#define NUM_DATA_SETS 5 ====== <br />
Number of statistics on which you are running the consensus estimator. This this particular case, 5. (Ix, Iy, Ixx, Ixy, Iyy)<br />
<br />
======#define NUMBERS_PER_SET 2======<br />
Number of variables in each data set (see above) that the consensus estimator needs to transmit to other agents. In this case, 2 because there is <tt>x_i</tt> and <tt>w_i</tt> for each statistic.<br />
<br />
======#define DATATYPE_BYTELENGTH 4 ======<br />
Number of bytes in the data type (used in the consensus estimator (float = 4 bytes long). This is important because we need to split the numbers into individual bytes to be able to send them out the serial port.<br />
<br />
======#define DATA_ARRAY_LENGTH (NUM_DATA_SETS*NUMBERS_PER_SET)======<br />
Total number of data variables needed for the consensus estimator. In this case, it is 5*2=10.<br />
<br />
======#define ADDITIONAL_NUMS 8======<br />
Additional number of data to be appended to data array. It is 5 in this case, so that we can append <br />
# Robot X coordinate<br />
# Robot Y coordinate<br />
# Robot Theta orientation<br />
# Robot left wheel speed<br />
# Robot right wheel speed<br />
# Sensor Red Value<br />
# Sensor Green Value<br />
# Sensor Blue Value<br />
<br />
===main===<br />
* .c: This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines. <br />
* .h: Contains variables, function prototypes, and delay function needed for main.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void)=====<br />
Timer1 ISR. Sets T1_INT_FLAG which provides timing for the main loop.<br />
<br />
=====void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)=====<br />
UART2 receive ISR. Runs with the XBee receives packets. The switch/case structure handles the packets.<br />
<br />
The current handling of coordinate data from the vision system: <br />
<br />
<code><pre><br />
case 0: //coordinate data<br />
e_set_configuration(<br />
InPacket.data[1].dataFloat,<br />
InPacket.data[2].dataFloat, <br />
InPacket.data[3].dataFloat);<br />
break;<br />
</pre></code><br />
<br />
Needs to be improved. Suggested new function:<br />
<br />
* Get vision system data<br />
* Check if it's wildly off from current puck estimate<br />
** If not, update, clear log of rejected packets<br />
** If so, reject and log<br />
*** If we have rejected enough packets (some threshold) and they've all been in similar place (within tolerance), we can assume the puck is wrong and vision system is right. In this case, update with vision system data and clear rejected data log.<br />
<br />
=====int main(void)=====<br />
Setup functions and main loop.<br />
<br />
===color_cal===<br />
* .c: Contains void calibrate_color(void) function to run the calibration routine.<br />
* .h: Contains function prototype and constant definitions for calibrate_color.<br />
<br />
=====void calibrate_color(void)=====<br />
This function runs the color calibration routine. Eventually this should be turned into a separate e-puck command from the vision system. Currently it just runs the puck through a zig zag pattern on the floor and sends packets. You can collect these with the data logger and process them in matlab. From this, you should be able to get a calibration function.<br />
<br />
'''To do:'''<br />
* Add to the pattern. Need to collect more data.<br />
* Possibly have the puck store data, find a best fit, and create the calibration function on board.<br />
* Store the calibration function in the EEPROM (flash memory) so it's non-volatile. The coefficients of function will be specific to each puck, so it would be nice to not have to program each puck, each time you change the battery.<br />
<br />
===dsPIC_XBeePackets===<br />
* .c/.h: Contains functions and data structures for assembling and receiving XBee packets.<br />
<br />
===send_packet===<br />
* .c: Contains the void send_packet(void) function which fills an array with data and calls the needed XBee functions to send a packet.<br />
* .h: Contains function prototype.<br />
<br />
=====void send_packet(void)=====<br />
* Creates ''packet'' array.<br />
* Adds consensus estimator data to the array.<br />
* Adds robot statistics to the array.<br />
* Adds color sensor values to the array.<br />
* Asserts flow control line to stop XBee from sending<br />
* Calls assemblePacket to send the packet.<br />
* Deasserts flow control; Xbee can send again.<br />
<br />
<br />
The function e_get_acc_filtered returns a running average of the acc specified. Syntax is:<br />
<br />
return value = e_get_acc_filtered(acc_channel, number of samples to average)<br />
<br />
The number of samples averaged must be less than ACC_SAMP_NB as defined in e_ad_conv.h.<br />
<br />
<code><pre><br />
packet[i]=e_get_acc_filtered(2, 136); // red<br />
i++;<br />
packet[i]=e_get_acc_filtered(1, 136); // green<br />
i++;<br />
packet[i]=e_get_acc_filtered(0, 136); // blue<br />
</pre></code><br />
<br />
===PI_consensus_estimator===<br />
* .h: Contains functions and data structures for the PI consensus estimator. <br />
* This will probably be replaced by the algorithm for sensor consensus. <br />
<br />
===wheel_speed_coordinator===<br />
* .c: Contains functions for robot motion control<br />
* .h: Function prototypes and variabls.<br />
<br />
=====void wheelSpeed(int *vL, int *vR)=====<br />
Return needed wheel speeds for the inertial consensus estimator based on the group goal.<br />
<br />
=====void wheelSpeedSingleBot(float gotox, float gotoy, int *vL, int *vR)=====<br />
Return needed wheel speed to get this individual bot to (gotox, gotoy). It's a hacked fix. Should be replaced with the 3 step motion controller from NUtest.c.<br />
<br />
===e_acc===<br />
* .c: Functions for reading the accelerometers (which is the color sensor).<br />
* .h: Function prototypes.<br />
<br />
This is original e-puck library code with the following modifications:<br />
<br />
<code><pre><br />
//changed by Sam, July 10, default offset is 2000. we want 0 for RGB sensor.<br />
static int centre_z = 0; //zero value for z axe<br />
</pre></code><br />
<br />
=====int e_get_acc_filtered(unsigned int captor, unsigned int filter_size)=====<br />
<br />
===e_ad_conv===<br />
Set up the ADCs on the puck. Original e-puck library code, with the following modifications<br />
* .h: Define constants and functional prototypes<br />
<br />
MIC_SAMP_FREQ sets the baseline sampling frequency for the ADC, everything else must be a fraction of this. 16384 Hz is the highest possible.<br />
<code><pre><br />
#define MIC_SAMP_FREQ 16384.0 <br />
</pre></code><br />
<br />
ACC_PROX_SAMP_FREQ sets the sampling frequency for the accelerometers (color sensor). We found in testing that the puck become non-responsive with this set to 8192 Hz or 16384 Hz.<br />
<code><pre><br />
// sampling frequency for the accelerometres and proximetres<br />
//#define ACC_PROX_SAMP_FREQ 256.0 // WARNING: should be a fraction of MIC_SAMP_FREQ<br />
#define ACC_PROX_SAMP_FREQ 4096 // to ensure a good timing precision<br />
// So your options are: 1 2 4 8 16 32 64 128 <br />
// 256 512 1024 2048 4096 8192 16384<br />
</pre></code><br />
<br />
ACC_SAMP_NB is the number of samples to store. We can do an average of ''up to'' this many samples. This is set to 140 so we can average 136 samples, which is 4 projector periods.<br />
<code><pre><br />
#define ACC_SAMP_NB 140 // number of accelerometer samples to store<br />
</pre></code><br />
<br />
* .c: Functions and interrupt service routines for ADCs. Original e-puck library, no modifications.<br />
<br />
=====e_init_ad_scan(ALL_ADC)=====<br />
Call to setup ADC and have it work in the background. Use e_acc functions to access data.<br />
<br />
===e_init_port===<br />
* .c/.h: Initializes the ports on the e-puck. File is from the standard e-puck library. <br />
<br />
=====e_init_ports(void)=====<br />
This function sets up ports on the e-puck.<br />
<br />
===e_led===<br />
* .c/.h: This is a standard e-puck library file that contains functions for manipulating LEDs.<br />
<br />
=====void e_set_led(unsigned int led_number, unsigned int value)=====<br />
Set led_number (0-7) to value (0=off 1=on higher=inverse).<br />
<br />
[[Image:e-puck_LED_numbering.png|thumb|left]]<br />
<br />
<br clear='all'><br />
<br />
===e_motors_swarm===<br />
* .c/.h: This file is a modified version of the e_motors.h e-puck library file. This version keeps track of the robot's position and orientation, and the motor stepping function contains code to update the robot's position when the wheels turn. <br />
* The functions like e_rotate and e_translate have been removed and this version is not dependent on e_agenda. <br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T5Interrupt(void)=====<br />
Timer5 ISR, interrupt for left motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos + cval*LINSTEP;<br />
ypos = ypos + sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void __attribute__((interrupt, auto_psv, shadow)) _T4Interrupt(void)=====<br />
Timer4 ISR, interrupt for right motor. Controls the stepper motors.<br />
<br />
Updates the pucks x, y, theta estimate here: <br />
<br />
<code><pre><br />
// update robot's configuration estimate<br />
thetapos = thetapos - ANGSTEP;<br />
if (thetapos<-PI) thetapos += 2*PI;<br />
xpos = xpos - cval*LINSTEP;<br />
ypos = ypos - sval*LINSTEP;<br />
}<br />
</pre></code><br />
<br />
=====void e_init_motors(void)=====<br />
Call this function before other motor functions to initialize the motors.<br />
<br />
=====void e_set_speed_left(int motor_speed)/void e_set_speed_right(int motor_speed)=====<br />
Set the motor speed in steps/second.<br />
<br />
=====void e_get_configuration(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the center reference point.<br />
<br />
=====void e_set_configuration(float x, float y, float theta)=====<br />
Sets x, y, theta to values.<br />
<br />
=====void e_get_configuration_front(float *xptr, float *yptr, float *thetaptr)=====<br />
Updates variables with current x, t, and theta (position and orientation) of the front reference point (used for motor control).<br />
<br />
<br />
[[Category:SwarmRobotProject]]<br />
[[Category:e-puck]]</div>Sam Bobb