William Pedler / Mbed 2 deprecated AutoCalibrateModulo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

main.cpp@2:755b99cceb46, 2013-04-09 (annotated)

Committer:
pedlerw
Date:
Tue Apr 09 20:50:35 2013 +0000
Revision:
2:755b99cceb46
Parent:
1:839192bf53f3 
4/9/2013; William Pedler; Publish to troubleshoot iostream issues

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pedlerw 2:755b99cceb46 1 //AutoCalibrateModulo//
pedlerw 0:4e9d7f54220e 2 //Written by William Pedler 4/8/2013//
pedlerw 0:4e9d7f54220e 3 //This program is used to automatically calibrate and move the forcer linearly//
pedlerw 0:4e9d7f54220e 4
pedlerw 0:4e9d7f54220e 5 //Includes//
pedlerw 0:4e9d7f54220e 6 #include "mbed.h"
pedlerw 2:755b99cceb46 7 //#include <iostream>
pedlerw 2:755b99cceb46 8 using namespace std;
pedlerw 0:4e9d7f54220e 9
pedlerw 0:4e9d7f54220e 10 //Define//
pedlerw 0:4e9d7f54220e 11 #define MAXI 1.0
pedlerw 0:4e9d7f54220e 12 #define MINI 0.0
pedlerw 0:4e9d7f54220e 13 #define BIG 100 //This times 4 controls how long the sensors read for each position//
pedlerw 0:4e9d7f54220e 14 #define three 3.0
pedlerw 0:4e9d7f54220e 15 #define seven 7
pedlerw 0:4e9d7f54220e 16 #define length 42
pedlerw 0:4e9d7f54220e 17 #define SPEED 0.150
pedlerw 2:755b99cceb46 18 #define medium 0.75
pedlerw 0:4e9d7f54220e 19 #define slow 0.5
pedlerw 0:4e9d7f54220e 20
pedlerw 0:4e9d7f54220e 21 //Declar global variables//
pedlerw 0:4e9d7f54220e 22 float left_min[length] = {0};
pedlerw 0:4e9d7f54220e 23 float left_max[length] = {0};
pedlerw 0:4e9d7f54220e 24 float right_min[length] = {0};
pedlerw 0:4e9d7f54220e 25 float right_max[length] = {0};
pedlerw 0:4e9d7f54220e 26
pedlerw 0:4e9d7f54220e 27 //Labeling the Local file system//
pedlerw 0:4e9d7f54220e 28 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
pedlerw 0:4e9d7f54220e 29 //Labeling onboard LED's//
pedlerw 0:4e9d7f54220e 30 DigitalOut led1(LED1);
pedlerw 0:4e9d7f54220e 31 DigitalOut led2(LED2);
pedlerw 0:4e9d7f54220e 32 DigitalOut led3(LED3);
pedlerw 0:4e9d7f54220e 33 DigitalOut led4(LED4);
pedlerw 0:4e9d7f54220e 34 //Label linear coils//
pedlerw 0:4e9d7f54220e 35 DigitalOut linear0(p5); //coil 1
pedlerw 0:4e9d7f54220e 36 DigitalOut linear1(p6); //coil 2
pedlerw 0:4e9d7f54220e 37 DigitalOut linear2(p7); //coil 3
pedlerw 0:4e9d7f54220e 38 DigitalOut linear3(p8); //coil 4
pedlerw 0:4e9d7f54220e 39 DigitalOut linear4(p9); //coil 5
pedlerw 0:4e9d7f54220e 40 DigitalOut linear5(p10); //coil 6
pedlerw 0:4e9d7f54220e 41 DigitalOut linear6(p11); //coil 7
pedlerw 0:4e9d7f54220e 42 //Reduce noise by declaring all unused Analog pins as DigitalOut//
pedlerw 0:4e9d7f54220e 43 DigitalOut left15(p15);
pedlerw 0:4e9d7f54220e 44 DigitalOut left16(p16);
pedlerw 0:4e9d7f54220e 45 DigitalOut left17(p17);
pedlerw 0:4e9d7f54220e 46 DigitalOut left18(p18);
pedlerw 0:4e9d7f54220e 47 AnalogIn distanceR(p19); //Vout yellow GND black Vss red
pedlerw 0:4e9d7f54220e 48 AnalogIn distanceL(p20); //Vout yellow GND black Vss red
pedlerw 0:4e9d7f54220e 49 //Set USB//
pedlerw 2:755b99cceb46 50 Serial pc(USBTX, USBRX);
pedlerw 0:4e9d7f54220e 51
pedlerw 0:4e9d7f54220e 52 //Functions//
pedlerw 0:4e9d7f54220e 53 void move_all_left();
pedlerw 2:755b99cceb46 54 void move_all_right();
pedlerw 0:4e9d7f54220e 55 float get_Lsensor_min();
pedlerw 0:4e9d7f54220e 56 float get_Lsensor_max();
pedlerw 0:4e9d7f54220e 57 float get_Rsensor_min();
pedlerw 0:4e9d7f54220e 58 float get_Rsensor_max();
pedlerw 0:4e9d7f54220e 59 void move_one_right(int p);
pedlerw 0:4e9d7f54220e 60 void move_one_left(int p);
pedlerw 0:4e9d7f54220e 61 int get_position();
pedlerw 0:4e9d7f54220e 62
pedlerw 0:4e9d7f54220e 63 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 64 /////////////////////////////Main Program//////////////////////////////////////
pedlerw 0:4e9d7f54220e 65 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 66
pedlerw 0:4e9d7f54220e 67 /*//////////////////////Pseudo code for Main://////////////////////////////////
pedlerw 0:4e9d7f54220e 68 -Calibrate Motor
pedlerw 0:4e9d7f54220e 69 1. Move all the way to the LEFT side of the track.
pedlerw 0:4e9d7f54220e 70 2. Get max and mins from both distance sensors.
pedlerw 0:4e9d7f54220e 71 3. Store the max and mins in various arrays.
pedlerw 0:4e9d7f54220e 72 4. Move one position to the RIGHT and repeat for entire track.
pedlerw 0:4e9d7f54220e 73 5. Once the entire track has been recorded export to a txt file on the mbed.
pedlerw 0:4e9d7f54220e 74 Format for txt file:
pedlerw 0:4e9d7f54220e 75 Position # :Left Low :Left High :Right Low :Right High
pedlerw 0:4e9d7f54220e 76 0 :#### :#### :#### :####
pedlerw 0:4e9d7f54220e 77 1 (3 \t) :#### (2 \t)...
pedlerw 0:4e9d7f54220e 78 .
pedlerw 0:4e9d7f54220e 79 .
pedlerw 0:4e9d7f54220e 80 #EOF
pedlerw 0:4e9d7f54220e 81 -Operate Motor
pedlerw 0:4e9d7f54220e 82 1. Get current location
pedlerw 0:4e9d7f54220e 83 2. Get desired location
pedlerw 0:4e9d7f54220e 84 3. Move to location
pedlerw 0:4e9d7f54220e 85 4. Repeat
pedlerw 0:4e9d7f54220e 86 *//////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 87 //////////////////////////////////MAIN/////////////////////////////////////////
pedlerw 0:4e9d7f54220e 88 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 89 int main() {
pedlerw 0:4e9d7f54220e 90 //Declarations & Initializations//
pedlerw 0:4e9d7f54220e 91 int current_location = 0;
pedlerw 0:4e9d7f54220e 92 int desired_location = 0;
pedlerw 0:4e9d7f54220e 93 int difference = 0;
pedlerw 0:4e9d7f54220e 94
pedlerw 0:4e9d7f54220e 95 led1 = 0;
pedlerw 0:4e9d7f54220e 96 led2 = 0;
pedlerw 0:4e9d7f54220e 97 led3 = 0;
pedlerw 0:4e9d7f54220e 98 led4 = 0;
pedlerw 0:4e9d7f54220e 99
pedlerw 0:4e9d7f54220e 100 //Speed Control//
pedlerw 0:4e9d7f54220e 101 wait(three);
pedlerw 0:4e9d7f54220e 102
pedlerw 0:4e9d7f54220e 103 //1.Move all the way to the LEFT side of the track//
pedlerw 0:4e9d7f54220e 104 move_all_left();
pedlerw 0:4e9d7f54220e 105 //2.Get Sensor Data//
pedlerw 0:4e9d7f54220e 106 for(int i=0;i<length;i++){
pedlerw 0:4e9d7f54220e 107 //3.Store in various arrays
pedlerw 0:4e9d7f54220e 108 left_min[i] = get_Lsensor_min();
pedlerw 0:4e9d7f54220e 109 left_max[i] = get_Lsensor_max();
pedlerw 0:4e9d7f54220e 110 right_min[i] = get_Rsensor_min();
pedlerw 0:4e9d7f54220e 111 right_max[i] = get_Rsensor_max();
pedlerw 2:755b99cceb46 112 led4 = !led4;
pedlerw 0:4e9d7f54220e 113 //4.Move to the right one//
pedlerw 0:4e9d7f54220e 114 move_one_right(i);
pedlerw 0:4e9d7f54220e 115 wait(slow);
pedlerw 0:4e9d7f54220e 116 }//End of for
pedlerw 0:4e9d7f54220e 117
pedlerw 0:4e9d7f54220e 118 //5. Export to a txt file on the mbed//
pedlerw 0:4e9d7f54220e 119 FILE *fp = fopen("/local/distance.txt", "w"); //Open "distance_data.txt" on the local file system for writing
pedlerw 0:4e9d7f54220e 120 fprintf(fp,"Position#\t Left Min\t Left Max\t Right Min\t Right Max\r\n");
pedlerw 0:4e9d7f54220e 121 for(int i=0;i<length;i++){
pedlerw 0:4e9d7f54220e 122 fprintf(fp,"%d\t %f\t %f\t %f\t %f\r\n",i,left_min[i],left_max[i],right_min[i],right_max[i]);
pedlerw 0:4e9d7f54220e 123 /*
pedlerw 0:4e9d7f54220e 124 //Example File Handeling Code//
pedlerw 0:4e9d7f54220e 125 FILE *fp = fopen("/local/out.txt", "w"); // Open "out.txt" on the local file system for writing
pedlerw 0:4e9d7f54220e 126 fprintf(fp, "Hello World!");
pedlerw 0:4e9d7f54220e 127 fclose(fp);
pedlerw 0:4e9d7f54220e 128 */
pedlerw 0:4e9d7f54220e 129 }//End of for
pedlerw 0:4e9d7f54220e 130 fclose(fp);
pedlerw 0:4e9d7f54220e 131
pedlerw 0:4e9d7f54220e 132 //Set an LED high so we know calibration is complete//
pedlerw 2:755b99cceb46 133 led1 = 1;
pedlerw 2:755b99cceb46 134 move_all_left();
pedlerw 2:755b99cceb46 135 //move_all_right();
pedlerw 2:755b99cceb46 136 //move_all_left();
pedlerw 0:4e9d7f54220e 137 //////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 138 ////////////////////////////Operate Motor/////////////////////////////////////
pedlerw 0:4e9d7f54220e 139 //////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 140
pedlerw 0:4e9d7f54220e 141 //Now use calibration data to operate motor//
pedlerw 0:4e9d7f54220e 142 while(1){
pedlerw 0:4e9d7f54220e 143 //1.Find current location//
pedlerw 2:755b99cceb46 144 current_location = get_position();
pedlerw 2:755b99cceb46 145 //2.Take in desired location//
pedlerw 0:4e9d7f54220e 146 //I'll code this later... for now go to position 13//
pedlerw 2:755b99cceb46 147 desired_location = 13;
pedlerw 2:755b99cceb46 148 /*/////////////////////////////////////////////////////////////////////////////
pedlerw 2:755b99cceb46 149 /////////////////////////////User Interface////////////////////////////////////
pedlerw 2:755b99cceb46 150 ///////////////////////////////////////////////////////////////////////////////
pedlerw 2:755b99cceb46 151 cout << "\n\n\rEnter a position number from 0 to 41: ";
pedlerw 2:755b99cceb46 152 cin >> desired_location;
pedlerw 2:755b99cceb46 153 while(std::cin.fail()){
pedlerw 2:755b99cceb46 154 cin.clear();
pedlerw 2:755b99cceb46 155 cin.ignore();
pedlerw 2:755b99cceb46 156 cout << "\n\n\r\t\tNot a number!\n\n\r\t\tPlease Try Again";
pedlerw 2:755b99cceb46 157 cout << "\n\rEnter a position number from 0 to 41: ";
pedlerw 2:755b99cceb46 158 cin >> desired_location;
pedlerw 2:755b99cceb46 159 }//End of while(std::cin.fail())//
pedlerw 2:755b99cceb46 160 cout << "\n\n\rGoing to position " << desired_location;
pedlerw 2:755b99cceb46 161 */
pedlerw 0:4e9d7f54220e 162 //3.Move to desired position//
pedlerw 2:755b99cceb46 163 while(current_location != desired_location){
pedlerw 0:4e9d7f54220e 164 difference = current_location - desired_location;
pedlerw 2:755b99cceb46 165 while(difference > 0){
pedlerw 2:755b99cceb46 166 //Move LEFT//
pedlerw 2:755b99cceb46 167 led2 = 1;
pedlerw 2:755b99cceb46 168 wait(medium);
pedlerw 2:755b99cceb46 169 for(int i=0; i<difference;i++){
pedlerw 2:755b99cceb46 170 move_one_left(abs(current_location));
pedlerw 2:755b99cceb46 171 current_location--;
pedlerw 2:755b99cceb46 172 wait(SPEED);
pedlerw 2:755b99cceb46 173 }//End of for
pedlerw 2:755b99cceb46 174 current_location = get_position();
pedlerw 2:755b99cceb46 175 difference = current_location - desired_location;
pedlerw 2:755b99cceb46 176 pc.printf("\r\n\nMoving Left");
pedlerw 2:755b99cceb46 177 pc.printf("\r\nCurrent Location:\t%d",current_location);
pedlerw 2:755b99cceb46 178 pc.printf("\r\nDesired Location:\t%d",desired_location);
pedlerw 2:755b99cceb46 179 pc.printf("\r\nDifference:\t%d",difference);
pedlerw 2:755b99cceb46 180 led2 = 0;
pedlerw 2:755b99cceb46 181 }//End of while(greater than zero)//
pedlerw 2:755b99cceb46 182 while(difference < 0){
pedlerw 2:755b99cceb46 183 led3 = 1;
pedlerw 2:755b99cceb46 184 wait(medium);
pedlerw 2:755b99cceb46 185 for(int i=0; i<abs(difference);i++){
pedlerw 2:755b99cceb46 186 move_one_right(current_location);
pedlerw 2:755b99cceb46 187 current_location++;
pedlerw 2:755b99cceb46 188 wait(SPEED);
pedlerw 2:755b99cceb46 189 }//End of for
pedlerw 2:755b99cceb46 190 current_location = get_position();
pedlerw 2:755b99cceb46 191 difference = current_location - desired_location;
pedlerw 2:755b99cceb46 192 pc.printf("\r\n\nMoving Right");
pedlerw 2:755b99cceb46 193 pc.printf("\r\nCurrent Location:\t%d",current_location);
pedlerw 2:755b99cceb46 194 pc.printf("\r\nDesired Location:\t%d",desired_location);
pedlerw 2:755b99cceb46 195 pc.printf("\r\nDifference:\t%d",difference);
pedlerw 2:755b99cceb46 196 led3 = 0;
pedlerw 2:755b99cceb46 197 }//End of while(difference less than 0)//
pedlerw 2:755b99cceb46 198 }//End of while(current not desired)//
pedlerw 2:755b99cceb46 199 //4.Repeat//
pedlerw 2:755b99cceb46 200 }//End of while(1)//
pedlerw 0:4e9d7f54220e 201 }//End of main
pedlerw 0:4e9d7f54220e 202
pedlerw 0:4e9d7f54220e 203 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 204 /////////////////////////////FUNCITONS/////////////////////////////////////////
pedlerw 0:4e9d7f54220e 205 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 206
pedlerw 0:4e9d7f54220e 207 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 208 ////////////////////////////move_all_left//////////////////////////////////////
pedlerw 0:4e9d7f54220e 209 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 210 //moves the forcer all the way to the left of the track//
pedlerw 0:4e9d7f54220e 211 void move_all_left(){
pedlerw 0:4e9d7f54220e 212 //Declartions and initialization//
pedlerw 0:4e9d7f54220e 213 float lng = (SPEED);
pedlerw 0:4e9d7f54220e 214 linear0 = 1; //Coil1 Mod0
pedlerw 0:4e9d7f54220e 215 linear1 = 1; //Coil2 Mod1
pedlerw 0:4e9d7f54220e 216 linear2 = 1; //Coil3 Mod2
pedlerw 0:4e9d7f54220e 217 linear3 = 1; //Coil4 Mod3
pedlerw 0:4e9d7f54220e 218 linear4 = 1; //Coil5 Mod4
pedlerw 0:4e9d7f54220e 219 linear5 = 1; //Coil6 Mod5
pedlerw 0:4e9d7f54220e 220 linear6 = 1; //Coil7 Mod6
pedlerw 0:4e9d7f54220e 221
pedlerw 0:4e9d7f54220e 222 //LEFT//
pedlerw 0:4e9d7f54220e 223 for(int i=0;i<6;i++){
pedlerw 0:4e9d7f54220e 224 linear0 =! linear0; //Coil 1
pedlerw 0:4e9d7f54220e 225 wait(lng);
pedlerw 0:4e9d7f54220e 226 linear0 =! linear0;
pedlerw 0:4e9d7f54220e 227 linear1 =! linear1; //Coil 2
pedlerw 0:4e9d7f54220e 228 wait(lng);
pedlerw 0:4e9d7f54220e 229 linear1 =! linear1;
pedlerw 0:4e9d7f54220e 230 linear2 =! linear2; //Coil 3
pedlerw 0:4e9d7f54220e 231 wait(lng);
pedlerw 0:4e9d7f54220e 232 linear2 =! linear2;
pedlerw 0:4e9d7f54220e 233 linear3 =! linear3; //Coil 4
pedlerw 0:4e9d7f54220e 234 wait(lng);
pedlerw 0:4e9d7f54220e 235 linear3 =! linear3;
pedlerw 0:4e9d7f54220e 236 linear4 =! linear4; //Coil 5
pedlerw 0:4e9d7f54220e 237 wait(lng);
pedlerw 0:4e9d7f54220e 238 linear4 =! linear4;
pedlerw 0:4e9d7f54220e 239 linear5 =! linear5; //Coil 6
pedlerw 0:4e9d7f54220e 240 wait(lng);
pedlerw 0:4e9d7f54220e 241 linear5 =! linear5;
pedlerw 0:4e9d7f54220e 242 linear6 =! linear6; //Coil 7
pedlerw 0:4e9d7f54220e 243 wait(lng);
pedlerw 0:4e9d7f54220e 244 linear6 =! linear6;
pedlerw 0:4e9d7f54220e 245 }//End of For loop
pedlerw 0:4e9d7f54220e 246 return;
pedlerw 0:4e9d7f54220e 247 }//End of Funciton
pedlerw 0:4e9d7f54220e 248
pedlerw 0:4e9d7f54220e 249 ///////////////////////////////////////////////////////////////////////////////
pedlerw 2:755b99cceb46 250 ////////////////////////////move_all_right/////////////////////////////////////
pedlerw 2:755b99cceb46 251 ///////////////////////////////////////////////////////////////////////////////
pedlerw 2:755b99cceb46 252 //moves the forcer all the way to the left of the track//
pedlerw 2:755b99cceb46 253 void move_all_right(){
pedlerw 2:755b99cceb46 254 //Declartions and initialization//
pedlerw 2:755b99cceb46 255 float lng = (SPEED);
pedlerw 2:755b99cceb46 256 linear0 = 1; //Coil1 Mod0
pedlerw 2:755b99cceb46 257 linear1 = 1; //Coil2 Mod1
pedlerw 2:755b99cceb46 258 linear2 = 1; //Coil3 Mod2
pedlerw 2:755b99cceb46 259 linear3 = 1; //Coil4 Mod3
pedlerw 2:755b99cceb46 260 linear4 = 1; //Coil5 Mod4
pedlerw 2:755b99cceb46 261 linear5 = 1; //Coil6 Mod5
pedlerw 2:755b99cceb46 262 linear6 = 1; //Coil7 Mod6
pedlerw 2:755b99cceb46 263
pedlerw 2:755b99cceb46 264 //RIGHT//
pedlerw 2:755b99cceb46 265 for(int i=0;i<6;i++){
pedlerw 2:755b99cceb46 266 linear6 =! linear6; //Coil 7
pedlerw 2:755b99cceb46 267 wait(lng);
pedlerw 2:755b99cceb46 268 linear6 =! linear6;
pedlerw 2:755b99cceb46 269 linear5 =! linear5; //Coil 6
pedlerw 2:755b99cceb46 270 wait(lng);
pedlerw 2:755b99cceb46 271 linear5 =! linear5;
pedlerw 2:755b99cceb46 272 linear4 =! linear4; //Coil 5
pedlerw 2:755b99cceb46 273 wait(lng);
pedlerw 2:755b99cceb46 274 linear4 =! linear4;
pedlerw 2:755b99cceb46 275 linear3 =! linear3; //Coil 4
pedlerw 2:755b99cceb46 276 wait(lng);
pedlerw 2:755b99cceb46 277 linear3 =! linear3;
pedlerw 2:755b99cceb46 278 linear2 =! linear2; //Coil 3
pedlerw 2:755b99cceb46 279 wait(lng);
pedlerw 2:755b99cceb46 280 linear2 =! linear2;
pedlerw 2:755b99cceb46 281 linear1 =! linear1; //Coil 2
pedlerw 2:755b99cceb46 282 wait(lng);
pedlerw 2:755b99cceb46 283 linear1 =! linear1;
pedlerw 2:755b99cceb46 284 linear0 =! linear0; //Coil 1
pedlerw 2:755b99cceb46 285 wait(lng);
pedlerw 2:755b99cceb46 286 linear0 =! linear0;
pedlerw 2:755b99cceb46 287 }//End of For loop
pedlerw 2:755b99cceb46 288 return;
pedlerw 2:755b99cceb46 289 }//End of Funciton
pedlerw 2:755b99cceb46 290
pedlerw 2:755b99cceb46 291 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 292 ////////////////////////////get_Lsensor_min////////////////////////////////////
pedlerw 0:4e9d7f54220e 293 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 294 //returns the min distance value of left sensor when called//
pedlerw 0:4e9d7f54220e 295 float get_Lsensor_min(){
pedlerw 0:4e9d7f54220e 296 //Declarations and initializations//
pedlerw 0:4e9d7f54220e 297 float min = MAXI;
pedlerw 0:4e9d7f54220e 298 float read;
pedlerw 0:4e9d7f54220e 299 for(int i=0;i<BIG;i++){
pedlerw 0:4e9d7f54220e 300 wait(0.005);
pedlerw 0:4e9d7f54220e 301 read = distanceL.read();
pedlerw 0:4e9d7f54220e 302 if(read < min){
pedlerw 0:4e9d7f54220e 303 min = read;
pedlerw 0:4e9d7f54220e 304 }//End of if
pedlerw 0:4e9d7f54220e 305 else{
pedlerw 0:4e9d7f54220e 306 wait(MINI);
pedlerw 0:4e9d7f54220e 307 }//End of else
pedlerw 0:4e9d7f54220e 308 }//End of for
pedlerw 0:4e9d7f54220e 309 return(min);
pedlerw 0:4e9d7f54220e 310 }//End of Function
pedlerw 0:4e9d7f54220e 311
pedlerw 0:4e9d7f54220e 312 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 313 //////////////////////////////get_Lsensor_max//////////////////////////////////
pedlerw 0:4e9d7f54220e 314 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 315 //returns the max distance value of the left sensor when called//
pedlerw 0:4e9d7f54220e 316 float get_Lsensor_max(){
pedlerw 0:4e9d7f54220e 317 //Declarations and initializations//
pedlerw 0:4e9d7f54220e 318 float max = MINI;
pedlerw 0:4e9d7f54220e 319 float read;
pedlerw 0:4e9d7f54220e 320 for(int i=0;i<BIG;i++){
pedlerw 0:4e9d7f54220e 321 wait(0.005);
pedlerw 0:4e9d7f54220e 322 read = distanceL.read();
pedlerw 0:4e9d7f54220e 323 if(read > max){
pedlerw 0:4e9d7f54220e 324 max = read;
pedlerw 0:4e9d7f54220e 325 }//End of if
pedlerw 0:4e9d7f54220e 326 else{
pedlerw 0:4e9d7f54220e 327 wait(MINI);
pedlerw 0:4e9d7f54220e 328 }//End of else
pedlerw 0:4e9d7f54220e 329 }//End of for
pedlerw 0:4e9d7f54220e 330 return(max);
pedlerw 0:4e9d7f54220e 331 }//End of function
pedlerw 0:4e9d7f54220e 332
pedlerw 0:4e9d7f54220e 333 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 334 /////////////////////////////get_Rsensor_min///////////////////////////////////
pedlerw 0:4e9d7f54220e 335 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 336 //Function returns the min value of the right sensor when called//
pedlerw 0:4e9d7f54220e 337 float get_Rsensor_min(){
pedlerw 0:4e9d7f54220e 338 //Declarations and initializations//
pedlerw 0:4e9d7f54220e 339 float min = MAXI;
pedlerw 0:4e9d7f54220e 340 float read;
pedlerw 0:4e9d7f54220e 341 for(int i=0;i<BIG;i++){
pedlerw 0:4e9d7f54220e 342 wait(0.005);
pedlerw 0:4e9d7f54220e 343 read = distanceR.read();
pedlerw 0:4e9d7f54220e 344 if(read < min){
pedlerw 0:4e9d7f54220e 345 min = read;
pedlerw 0:4e9d7f54220e 346 }//End of if
pedlerw 0:4e9d7f54220e 347 else{
pedlerw 0:4e9d7f54220e 348 wait(MINI);
pedlerw 0:4e9d7f54220e 349 }//End of else
pedlerw 0:4e9d7f54220e 350 }//End of for
pedlerw 0:4e9d7f54220e 351 return(min);
pedlerw 0:4e9d7f54220e 352 }//End of function
pedlerw 0:4e9d7f54220e 353
pedlerw 0:4e9d7f54220e 354 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 355 ///////////////////////////////get_Rsensor_max/////////////////////////////////
pedlerw 0:4e9d7f54220e 356 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 357 float get_Rsensor_max(){
pedlerw 0:4e9d7f54220e 358 //Declarations and initializations//
pedlerw 0:4e9d7f54220e 359 float max = MINI;
pedlerw 0:4e9d7f54220e 360 float read;
pedlerw 0:4e9d7f54220e 361 for(int i=0;i<BIG;i++){
pedlerw 0:4e9d7f54220e 362 wait(0.005);
pedlerw 0:4e9d7f54220e 363 read = distanceR.read();
pedlerw 0:4e9d7f54220e 364 if(read > max){
pedlerw 0:4e9d7f54220e 365 max = read;
pedlerw 0:4e9d7f54220e 366 }//End of if
pedlerw 0:4e9d7f54220e 367 else{
pedlerw 0:4e9d7f54220e 368 wait(MINI);
pedlerw 0:4e9d7f54220e 369 }//End of else
pedlerw 0:4e9d7f54220e 370 }//End of for
pedlerw 0:4e9d7f54220e 371 return(max);
pedlerw 0:4e9d7f54220e 372 }//End of function
pedlerw 0:4e9d7f54220e 373
pedlerw 0:4e9d7f54220e 374 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 375 ///////////////////////////////move_one_right//////////////////////////////////
pedlerw 0:4e9d7f54220e 376 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 377 //Function takes current position and moves one to the right
pedlerw 0:4e9d7f54220e 378 void move_one_right(int position){
pedlerw 0:4e9d7f54220e 379 //Declarations and initializations//
pedlerw 0:4e9d7f54220e 380 int current_coil = position % seven;
pedlerw 2:755b99cceb46 381 pc.printf("\r\nCurrent Coil:\t%d",current_coil);
pedlerw 0:4e9d7f54220e 382 switch(current_coil){
pedlerw 0:4e9d7f54220e 383 case 0:
pedlerw 0:4e9d7f54220e 384 linear6 = 1;
pedlerw 0:4e9d7f54220e 385 linear5 = 0; //Fire Coil 6//
pedlerw 0:4e9d7f54220e 386 linear4 = 1;
pedlerw 0:4e9d7f54220e 387 linear3 = 1;
pedlerw 0:4e9d7f54220e 388 linear2 = 1;
pedlerw 0:4e9d7f54220e 389 linear1 = 1;
pedlerw 0:4e9d7f54220e 390 linear0 = 1;
pedlerw 0:4e9d7f54220e 391 break;
pedlerw 0:4e9d7f54220e 392 case 1:
pedlerw 0:4e9d7f54220e 393 linear6 = 1;
pedlerw 0:4e9d7f54220e 394 linear5 = 1;
pedlerw 0:4e9d7f54220e 395 linear4 = 0; //Fire Coil 5//
pedlerw 0:4e9d7f54220e 396 linear3 = 1;
pedlerw 0:4e9d7f54220e 397 linear2 = 1;
pedlerw 0:4e9d7f54220e 398 linear1 = 1;
pedlerw 0:4e9d7f54220e 399 linear0 = 1;
pedlerw 0:4e9d7f54220e 400 break;
pedlerw 0:4e9d7f54220e 401 case 2:
pedlerw 0:4e9d7f54220e 402 linear6 = 1;
pedlerw 0:4e9d7f54220e 403 linear5 = 1;
pedlerw 0:4e9d7f54220e 404 linear4 = 1;
pedlerw 0:4e9d7f54220e 405 linear3 = 0; //Fire Coil 4//
pedlerw 0:4e9d7f54220e 406 linear2 = 1;
pedlerw 0:4e9d7f54220e 407 linear1 = 1;
pedlerw 0:4e9d7f54220e 408 linear0 = 1;
pedlerw 0:4e9d7f54220e 409 break;
pedlerw 0:4e9d7f54220e 410 case 3:
pedlerw 0:4e9d7f54220e 411 linear6 = 1;
pedlerw 0:4e9d7f54220e 412 linear5 = 1;
pedlerw 0:4e9d7f54220e 413 linear4 = 1;
pedlerw 0:4e9d7f54220e 414 linear3 = 1;
pedlerw 0:4e9d7f54220e 415 linear2 = 0; //Fire Coil 3//
pedlerw 0:4e9d7f54220e 416 linear1 = 1;
pedlerw 0:4e9d7f54220e 417 linear0 = 1;
pedlerw 0:4e9d7f54220e 418 break;
pedlerw 0:4e9d7f54220e 419 case 4:
pedlerw 0:4e9d7f54220e 420 linear6 = 1;
pedlerw 0:4e9d7f54220e 421 linear5 = 1;
pedlerw 0:4e9d7f54220e 422 linear4 = 1;
pedlerw 0:4e9d7f54220e 423 linear3 = 1;
pedlerw 0:4e9d7f54220e 424 linear2 = 1;
pedlerw 0:4e9d7f54220e 425 linear1 = 0; //Fire Coil 2//
pedlerw 0:4e9d7f54220e 426 linear0 = 1;
pedlerw 0:4e9d7f54220e 427 break;
pedlerw 0:4e9d7f54220e 428 case 5:
pedlerw 0:4e9d7f54220e 429 linear6 = 1;
pedlerw 0:4e9d7f54220e 430 linear5 = 1;
pedlerw 0:4e9d7f54220e 431 linear4 = 1;
pedlerw 0:4e9d7f54220e 432 linear3 = 1;
pedlerw 0:4e9d7f54220e 433 linear2 = 1;
pedlerw 0:4e9d7f54220e 434 linear1 = 1;
pedlerw 0:4e9d7f54220e 435 linear0 = 0; //Fire Coil 1//
pedlerw 0:4e9d7f54220e 436 break;
pedlerw 0:4e9d7f54220e 437 case 6:
pedlerw 0:4e9d7f54220e 438 linear6 = 0; //Fire Coil 7//
pedlerw 0:4e9d7f54220e 439 linear5 = 1;
pedlerw 0:4e9d7f54220e 440 linear4 = 1;
pedlerw 0:4e9d7f54220e 441 linear3 = 1;
pedlerw 0:4e9d7f54220e 442 linear2 = 1;
pedlerw 0:4e9d7f54220e 443 linear1 = 1;
pedlerw 0:4e9d7f54220e 444 linear0 = 1;
pedlerw 0:4e9d7f54220e 445 break;
pedlerw 0:4e9d7f54220e 446 default:
pedlerw 0:4e9d7f54220e 447 wait(MINI);
pedlerw 0:4e9d7f54220e 448 }//End of switch
pedlerw 0:4e9d7f54220e 449 }//End of function
pedlerw 0:4e9d7f54220e 450
pedlerw 0:4e9d7f54220e 451 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 452 ///////////////////////////////move_one_left//////////////////////////////////
pedlerw 0:4e9d7f54220e 453 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 454 //Function takes current position and moves one to the left
pedlerw 0:4e9d7f54220e 455 void move_one_left(int position){
pedlerw 0:4e9d7f54220e 456 //Declarations and initializations//
pedlerw 0:4e9d7f54220e 457 int current_coil = position % seven;
pedlerw 2:755b99cceb46 458 pc.printf("\r\nCurrent Coil:\t%d",current_coil);
pedlerw 0:4e9d7f54220e 459 switch(current_coil){
pedlerw 0:4e9d7f54220e 460 case 0:
pedlerw 0:4e9d7f54220e 461 linear6 = 1;
pedlerw 0:4e9d7f54220e 462 linear5 = 1;
pedlerw 0:4e9d7f54220e 463 linear4 = 1;
pedlerw 0:4e9d7f54220e 464 linear3 = 1;
pedlerw 0:4e9d7f54220e 465 linear2 = 1;
pedlerw 0:4e9d7f54220e 466 linear1 = 1;
pedlerw 0:4e9d7f54220e 467 linear0 = 0; //Fire Coil 1//
pedlerw 0:4e9d7f54220e 468 break;
pedlerw 0:4e9d7f54220e 469 case 1:
pedlerw 0:4e9d7f54220e 470 linear6 = 0; //Fire Coil 7//
pedlerw 0:4e9d7f54220e 471 linear5 = 1;
pedlerw 0:4e9d7f54220e 472 linear4 = 1;
pedlerw 0:4e9d7f54220e 473 linear3 = 1;
pedlerw 0:4e9d7f54220e 474 linear2 = 1;
pedlerw 0:4e9d7f54220e 475 linear1 = 1;
pedlerw 0:4e9d7f54220e 476 linear0 = 1;
pedlerw 0:4e9d7f54220e 477 break;
pedlerw 0:4e9d7f54220e 478 case 2:
pedlerw 0:4e9d7f54220e 479 linear6 = 1;
pedlerw 0:4e9d7f54220e 480 linear5 = 0; //Fire Coil 6//
pedlerw 0:4e9d7f54220e 481 linear4 = 1;
pedlerw 0:4e9d7f54220e 482 linear3 = 1;
pedlerw 0:4e9d7f54220e 483 linear2 = 1;
pedlerw 0:4e9d7f54220e 484 linear1 = 1;
pedlerw 0:4e9d7f54220e 485 linear0 = 1;
pedlerw 0:4e9d7f54220e 486 break;
pedlerw 0:4e9d7f54220e 487 case 3:
pedlerw 0:4e9d7f54220e 488 linear6 = 1;
pedlerw 0:4e9d7f54220e 489 linear5 = 1;
pedlerw 0:4e9d7f54220e 490 linear4 = 0; //Fire Coil 4//
pedlerw 0:4e9d7f54220e 491 linear3 = 1;
pedlerw 0:4e9d7f54220e 492 linear2 = 1;
pedlerw 0:4e9d7f54220e 493 linear1 = 1;
pedlerw 0:4e9d7f54220e 494 linear0 = 1;
pedlerw 0:4e9d7f54220e 495 break;
pedlerw 0:4e9d7f54220e 496 case 4:
pedlerw 0:4e9d7f54220e 497 linear6 = 1;
pedlerw 0:4e9d7f54220e 498 linear5 = 1;
pedlerw 0:4e9d7f54220e 499 linear4 = 1;
pedlerw 0:4e9d7f54220e 500 linear3 = 0; //Fire Coil 4//
pedlerw 0:4e9d7f54220e 501 linear2 = 1;
pedlerw 0:4e9d7f54220e 502 linear1 = 1;
pedlerw 0:4e9d7f54220e 503 linear0 = 1;
pedlerw 0:4e9d7f54220e 504 break;
pedlerw 0:4e9d7f54220e 505 case 5:
pedlerw 0:4e9d7f54220e 506 linear6 = 1;
pedlerw 0:4e9d7f54220e 507 linear5 = 1;
pedlerw 0:4e9d7f54220e 508 linear4 = 1;
pedlerw 0:4e9d7f54220e 509 linear3 = 1;
pedlerw 0:4e9d7f54220e 510 linear2 = 0; //Fire Coil 3//
pedlerw 0:4e9d7f54220e 511 linear1 = 1;
pedlerw 0:4e9d7f54220e 512 linear0 = 1;
pedlerw 0:4e9d7f54220e 513 break;
pedlerw 0:4e9d7f54220e 514 case 6:
pedlerw 0:4e9d7f54220e 515 linear6 = 1;
pedlerw 0:4e9d7f54220e 516 linear5 = 1;
pedlerw 0:4e9d7f54220e 517 linear4 = 1;
pedlerw 0:4e9d7f54220e 518 linear3 = 1;
pedlerw 0:4e9d7f54220e 519 linear2 = 1;
pedlerw 0:4e9d7f54220e 520 linear1 = 0; //Fire Coil 2//
pedlerw 0:4e9d7f54220e 521 linear0 = 1;
pedlerw 0:4e9d7f54220e 522 break;
pedlerw 0:4e9d7f54220e 523 default:
pedlerw 0:4e9d7f54220e 524 wait(MINI);
pedlerw 0:4e9d7f54220e 525 }//End of switch
pedlerw 0:4e9d7f54220e 526 }//End of function
pedlerw 0:4e9d7f54220e 527
pedlerw 0:4e9d7f54220e 528 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 529 /////////////////////////////get_position//////////////////////////////////////
pedlerw 0:4e9d7f54220e 530 ///////////////////////////////////////////////////////////////////////////////
pedlerw 0:4e9d7f54220e 531
pedlerw 0:4e9d7f54220e 532 //Function reads current position on track and returns the mod 7//
pedlerw 0:4e9d7f54220e 533 //Function uses calibration data to determin current position//
pedlerw 0:4e9d7f54220e 534 int get_position(){
pedlerw 0:4e9d7f54220e 535 //Declrations and Initializaitons//
pedlerw 0:4e9d7f54220e 536 int position = 0;
pedlerw 0:4e9d7f54220e 537 float left_sensor = distanceL.read();
pedlerw 0:4e9d7f54220e 538 float right_sensor = distanceR.read();
pedlerw 0:4e9d7f54220e 539 //Use closest sensor//
pedlerw 0:4e9d7f54220e 540 if(left_sensor > right_sensor){
pedlerw 0:4e9d7f54220e 541 //Look at Lsensor data//
pedlerw 0:4e9d7f54220e 542 for(int i=0;i<length;i++){
pedlerw 0:4e9d7f54220e 543 if(left_sensor < left_max[i] && left_sensor > left_min[i]){
pedlerw 1:839192bf53f3 544 position = i;
pedlerw 0:4e9d7f54220e 545 }//End of if//
pedlerw 0:4e9d7f54220e 546 }//End of for//
pedlerw 0:4e9d7f54220e 547 }//End of if//
pedlerw 0:4e9d7f54220e 548
pedlerw 0:4e9d7f54220e 549 else if(right_sensor >= left_sensor){
pedlerw 0:4e9d7f54220e 550 //Look at Rsensor data//
pedlerw 0:4e9d7f54220e 551 for(int i=0;i<length;i++){
pedlerw 0:4e9d7f54220e 552 if(right_sensor < right_max[i] && right_sensor > right_min[i]){
pedlerw 1:839192bf53f3 553 position = i;
pedlerw 0:4e9d7f54220e 554 }//End of if//
pedlerw 0:4e9d7f54220e 555 }//End of for//
pedlerw 0:4e9d7f54220e 556 }//End of else if//
pedlerw 0:4e9d7f54220e 557 return position;
pedlerw 0:4e9d7f54220e 558 }//End of function