Exploration 1 – Introduction to Motors and Sensors
- Code 1: Continuously Reading Data from CdS Cell
-
#include <FEHLCD.h> #include <FEHIO.h> #include <FEHUtility.h> AnalogInputPin CdS_cell(FEHIO::P1_0); int main(void) { LCD.Clear(FEHLCD::Black); while(true) { LCD.WriteRC( CdS_cell.Value(), 6, 10); } return 0; }
-
- Code 2: Continuous Control of Servo Motor using Varying Light Intensities
-
#include <FEHLCD.h> #include <FEHIO.h> #include <FEHUtility.h> #include <FEHServo.h> FEHServo servo(FEHServo::Servo0); AnalogInputPin CdS_cell(FEHIO::P1_0); int main(void) { //servo.TouchCalibrate(); servo.SetMin(500); servo.SetMax(2440); LCD.Clear(FEHLCD::Black); int degree; while(true) { LCD.WriteRC( CdS_cell.Value(), 6, 10); degree = (CdS_cell.Value() - 0.303) / (3.260 - 0.303) * 180; LCD.WriteRC(degree, 7, 10); LCD.WriteRC(" ", 7, 10); servo.SetDegree(degree); } return 0; }
-
- Code 3: Bump-Switch Navigation of Course
-
#include <FEHLCD.h> #include <FEHIO.h> #include <FEHMotor.h> DigitalInputPin microswitch1(FEHIO::P0_0); DigitalInputPin microswitch2(FEHIO::P0_1); DigitalInputPin microswitch3(FEHIO::P3_0); DigitalInputPin microswitch4(FEHIO::P3_1); FEHMotor left_motor(FEHMotor::Motor2, 12.0); FEHMotor right_motor(FEHMotor::Motor1, 12.0); int main(void) { LCD.Clear( FEHLCD::Black ); LCD.SetFontColor( FEHLCD::White ); LCD.WriteLine("Waiting for a button press..."); while( 4 == (microswitch1.Value() + microswitch2.Value() + microswitch3.Value() + microswitch4.Value()) ); Sleep(0.5); LCD.WriteLine("Button press recieved"); LCD.WriteLine("Beginning course..."); LCD.WriteLine("Straight forward"); left_motor.SetPercent(40.0); right_motor.SetPercent(40.0); while( 0 != (microswitch2.Value() + microswitch3.Value()) ); LCD.WriteLine("Backwards left turn"); left_motor.Stop(); right_motor.Stop(); LCD.WriteLine("Pausing..."); Sleep(0.25); LCD.WriteLine("Backwards right"); right_motor.SetPercent(-40.0); while( microswitch1.Value() ); right_motor.Stop(); left_motor.Stop(); Sleep(0.25); LCD.WriteLine("Aligning"); left_motor.SetPercent(-30); while( microswitch4.Value() ); LCD.WriteLine("Going backwards"); left_motor.SetPercent(-20.0); right_motor.SetPercent(-20.0); while( 0 != (microswitch1.Value() + microswitch4.Value()) ); left_motor.Stop(); right_motor.Stop(); Sleep(0.25); //align with wall LCD.WriteLine("Straight forward"); left_motor.SetPercent(40.0); right_motor.SetPercent(40.0); Sleep(0.25); while( 0 != (microswitch2.Value() + microswitch3.Value() )); left_motor.Stop(); right_motor.Stop(); Sleep(0.25); LCD.WriteLine("Backwards right turn"); left_motor.SetPercent(-40.0); right_motor.Stop(); while( microswitch4.Value() ); LCD.WriteLine("Aligning"); left_motor.Stop(); right_motor.SetPercent(-30.0); while( microswitch1.Value() ); LCD.WriteLine("Going backwards"); left_motor.SetPercent(-20.0); right_motor.SetPercent(-20.0); while( 0 != (microswitch1.Value() + microswitch4.Value()) ); left_motor.Stop(); right_motor.Stop(); Sleep(0.25); //align with wall LCD.WriteLine("Straight forward"); left_motor.SetPercent(40.0); right_motor.SetPercent(40.0); while( 0 != (microswitch2.Value() + microswitch3.Value())); LCD.WriteLine("Course completed"); left_motor.Stop(); right_motor.Stop(); return 0; }
-
Exploration 2 – Line Following and Shaft Encoding
- Code 0: Determining the Reflectivity of the Strips for each Optosensor
-
#include <FEHLCD.h> #include <FEHIO.h> #include <FEHUtility.h> AnalogInputPin linefollower1(FEHIO::P1_2); AnalogInputPin linefollower2(FEHIO::P1_1); AnalogInputPin linefollower3(FEHIO::P1_0); int main(void) { while (true) { LCD.WriteRC(linefollower1.Value(), 3, 5); LCD.WriteRC(linefollower2.Value(), 4, 5); LCD.WriteRC(linefollower3.Value(), 5, 5); } return 0; }
-
- Code 1: Line Following with 3 Optosensor System
-
#include <FEHLCD.h> #include <FEHIO.h> #include <FEHUtility.h> #include <FEHMotor.h> #define NOT_ON_LINE 0 #define LEFT_OF_LINE 1 // #define SLIGHTLY_LEFT_OF_LINE 2 #define ON_LINE 3 #define SLIGHTLY_RIGHT_OF_LINE 4 #define RIGHT_OF_LINE 5 #define Opto1Line 2.53 #define Opto1NoLine 1.49 #define Opto2Line 2.54 #define Opto2NoLine 1.59 #define Opto3Line 2.60 #define Opto3NoLine 1.64 //Declarations for encoders & motors AnalogInputPin linefollower1(FEHIO::P1_2); AnalogInputPin linefollower2(FEHIO::P1_1); AnalogInputPin linefollower3(FEHIO::P1_0); FEHMotor leftmotor(FEHMotor::Motor0, 12.0); FEHMotor rightmotor(FEHMotor::Motor1, 12.0); int status; void sensor_read() { int leftopto = 0, middleopto = 0, rightopto = 0; if (linefollower1.Value() > Opto1Line - 0.4) leftopto = 1; if (linefollower2.Value() > Opto2Line - 0.4) middleopto = 1; if (linefollower3.Value() > Opto3Line - 0.4) rightopto = 1; if (leftopto == 1 && middleopto == 0 && rightopto == 0) status = 5; if (leftopto == 1 && middleopto == 1 && rightopto == 0) status = 4; if (leftopto == 0 && middleopto == 1 && rightopto == 0) status = 3; if (leftopto == 0 && middleopto == 1 && rightopto == 1) status = 2; if (leftopto == 0 && middleopto == 0 && rightopto == 1) status = 1; if (leftopto == 0 && middleopto == 0 && rightopto == 0) status = status; } void line_follow() { switch (status) { case NOT_ON_LINE: LCD.WriteRC("Not on the line", 2, 1); leftmotor.Stop(); rightmotor.Stop(); break; case LEFT_OF_LINE: LCD.WriteRC("Left of the line", 2, 1); leftmotor.SetPercent(30); rightmotor.SetPercent(10); break; case SLIGHTLY_LEFT_OF_LINE: LCD.WriteRC("Slightly left of line", 2, 1); leftmotor.SetPercent(30); rightmotor.SetPercent(20); break; case ON_LINE: LCD.WriteRC("On the line", 2, 1); leftmotor.SetPercent(30); rightmotor.SetPercent(30); break; case SLIGHTLY_RIGHT_OF_LINE: LCD.WriteRC("Slightly right of the line", 2, 1); leftmotor.SetPercent(20); rightmotor.SetPercent(30); break; case RIGHT_OF_LINE: LCD.WriteRC("Right of line", 2, 1); leftmotor.SetPercent(10); rightmotor.SetPercent(30); break; } } int main(void) { LCD.Clear(); int a=0; while(true) { LCD.WriteRC(a,1,1); sensor_read(); line_follow(); a++; } return 0; }
-
- Code 2: Using Shaft Encoding to Make 90° Turns and Driving Specified Distances
-
#include <FEHLCD.h> #include <FEHIO.h> #include <FEHUtility.h> #include <FEHMotor.h> #include <FEHRPS.h> //Declarations for encoders & motors DigitalEncoder left_encoder(FEHIO::P0_0); DigitalEncoder right_encoder(FEHIO::P0_1); FEHMotor left_motor(FEHMotor::Motor0, 12.0); FEHMotor right_motor(FEHMotor::Motor1, 12.0); DigitalInputPin bumpswitch(FEHIO::P2_0); void move_forward(int percent, int counts) //using encoders { //Reset encoder counts right_encoder.ResetCounts(); left_encoder.ResetCounts(); //Set both motors to desired percent right_motor.SetPercent(percent); left_motor.SetPercent(percent); //While the average of the left and right encoder are less than counts, //keep running motors while((left_encoder.Counts() + right_encoder.Counts()) / 2. < counts); //Turn off motors right_motor.Stop(); left_motor.Stop(); } void turn(int percent, int counts) //negative percent for left turn, positive for right { right_encoder.ResetCounts(); left_encoder.ResetCounts(); right_motor.SetPercent(-percent); left_motor.SetPercent(percent); while((left_encoder.Counts() + right_encoder.Counts()) / 2. < counts); right_motor.Stop(); left_motor.Stop(); } int main(void) { //Initialize the screen (GO BUCKS!) LCD.Clear( FEHLCD::Scarlet ); LCD.SetFontColor( FEHLCD::Gray ); LCD.WriteLine("Shaft Encoder Lab Test"); LCD.WriteLine("Press bumpswitch"); while(bumpswitch.Value() ); //Wait for bumpswitch move_forward(30, 546); Sleep(0.5); turn(-30, 210); Sleep(0.5); move_forward(30, 384); Sleep(0.5); turn(30, 210); Sleep(0.5); move_forward(30, 141); return 0; }
-
Exploration 3 – RPS and Data Logging
- Code 1:
-
#include <FEHLCD.h> <pre>#include <FEHIO.h> #include <FEHUtility.h> #include <FEHMotor.h> #include <FEHRPS.h> //Declarations for encoders & motors DigitalEncoder right_encoder(FEHIO::P0_1); DigitalEncoder left_encoder(FEHIO::P0_0); FEHMotor right_motor(FEHMotor::Motor1, 12.0); FEHMotor left_motor(FEHMotor::Motor0, 12.0); void move_forward(int percent, int counts) //using encoders { //Reset encoder counts right_encoder.ResetCounts(); left_encoder.ResetCounts(); //Set both motors to desired percent right_motor.SetPercent(percent); left_motor.SetPercent(percent); //While the average of the left and right encoder are less than counts, //keep running motors while((left_encoder.Counts() + right_encoder.Counts()) / 2. < counts); //Turn off motors right_motor.Stop(); left_motor.Stop(); } void turn_right(int percent, int counts) //using encoders { //Reset encoder counts right_encoder.ResetCounts(); left_encoder.ResetCounts(); //Set both motors to desired percent right_motor.SetPercent(-percent); left_motor.SetPercent(percent); //While the average of the left and right encoder are less than counts, //keep running motors while((left_encoder.Counts() + right_encoder.Counts()) / 2. < counts); //Turn off motors right_motor.Stop(); left_motor.Stop(); } void turn_left(int percent, int counts) //using encoders { //Reset encoder counts right_encoder.ResetCounts(); left_encoder.ResetCounts(); //Set both motors to desired percent right_motor.SetPercent(percent); left_motor.SetPercent(-percent); //While the average of the left and right encoder are less than counts, //keep running motors while((left_encoder.Counts() + right_encoder.Counts()) / 2. < counts); //Turn off motors right_motor.Stop(); left_motor.Stop(); } void check_x_plus(float x_coordinate) //using RPS while robot is in the +x direction { //check whether the robot is within an acceptable range while(RPS.X() < x_coordinate - 1 || RPS.X() > x_coordinate + 1) { if(RPS.X() > x_coordinate) { //pulse the motors for a short duration in the correct direction right_motor.SetPercent(10); left_motor.SetPercent(10); Sleep(10); right_motor.SetPercent(0); left_motor.SetPercent(0); } else if(RPS.X() < x_coordinate) { //pulse the motors for a short duration in the correct direction right_motor.SetPercent(-10); left_motor.SetPercent(-10); Sleep(10); right_motor.SetPercent(0); left_motor.SetPercent(0); } } } void check_y_minus(float y_coordinate) //using RPS while robot is in the -y direction { //check whether the robot is within an acceptable range while(RPS.Y() < y_coordinate - 1 || RPS.Y() > y_coordinate + 1) { if(RPS.Y() > y_coordinate) { //pulse the motors for a short duration in the correct direction right_motor.SetPercent(-10); left_motor.SetPercent(-10); Sleep(10); right_motor.SetPercent(0); left_motor.SetPercent(0); } else if(RPS.Y() < y_coordinate) { //pulse the motors for a short duration in the correct direction right_motor.SetPercent(10); left_motor.SetPercent(10); Sleep(10); right_motor.SetPercent(0); left_motor.SetPercent(0); } } } void check_y_plus(float y_coordinate) //using RPS while robot is in the +y direction { //check whether the robot is within an acceptable range while(RPS.Y() < y_coordinate - 1 || RPS.Y() > y_coordinate + 1) { if(RPS.Y() > y_coordinate) { //pulse the motors for a short duration in the correct direction right_motor.SetPercent(10); left_motor.SetPercent(10); Sleep(10); right_motor.SetPercent(0); left_motor.SetPercent(0); } else if(RPS.Y() < y_coordinate) { //pulse the motors for a short duration in the correct direction right_motor.SetPercent(-10); left_motor.SetPercent(-10); Sleep(10); right_motor.SetPercent(0); left_motor.SetPercent(0); } } } void check_heading(float heading) //using RPS { //you will need to fill out this one yourself and take into account //the edge conditions (when you want the robot to go to 0 degrees //or close to 0 degrees) while(RPS.Heading() < heading + 360*(heading==0) - 1 || RPS.Heading() > heading - 360*(heading==0) + 1 ) { LCD.WriteRC(RPS.Heading() ,5,5); if (RPS.Heading() < heading + 360*(heading==0) - 1 ) { left_motor.SetPercent(10); right_motor.SetPercent(-10); Sleep(10); left_motor.SetPercent(0); right_motor.SetPercent(0); } if (RPS.Heading() > heading) { left_motor.SetPercent(-10); right_motor.SetPercent(10); Sleep(15); left_motor.SetPercent(0); right_motor.SetPercent(0); } } } int main(void) { float touch_x,touch_y; RPS.InitializeTouchMenu(); LCD.WriteLine("RPS & Data Logging Test"); LCD.WriteLine("Press Screen To Start"); while(!LCD.Touch(&touch_x,&touch_y)); //Wait for touchscreen press move_forward(-30, 240); check_y_plus(19.5); turn_right(-30, 205); check_heading(270); move_forward(-30, 21 * 40 ); check_x_plus(28.8); turn_right(-30, 205); check_heading(180); move_forward(-30, 240); check_y_minus(11.7); check_heading(180); return 0; }
-
- Code 2 –
-
#include <FEHLCD.h> #include <FEHIO.h> #include <FEHUtility.h> #include <FEHMotor.h> #include <FEHRPS.h> #include <FEHSD.h> //Declarations for encoders & motors DigitalEncoder right_encoder(FEHIO::P0_1); DigitalEncoder left_encoder(FEHIO::P0_0); FEHMotor right_motor(FEHMotor::Motor1, 12.0); FEHMotor left_motor(FEHMotor::Motor0, 12.0); void move_forward(int percent, int counts) //using encoders { //Reset encoder counts right_encoder.ResetCounts(); left_encoder.ResetCounts(); //Set both motors to desired percent right_motor.SetPercent(percent); left_motor.SetPercent(percent); //While the average of the left and right encoder are less than counts, //keep running motors while((left_encoder.Counts() + right_encoder.Counts()) / 2. < counts); //Turn off motors right_motor.Stop(); left_motor.Stop(); } void turn_right(int percent, int counts) //using encoders { //Reset encoder counts right_encoder.ResetCounts(); left_encoder.ResetCounts(); //Set both motors to desired percent right_motor.SetPercent(-percent); left_motor.SetPercent(percent); //While the average of the left and right encoder are less than counts, //keep running motors while((left_encoder.Counts() + right_encoder.Counts()) / 2. < counts); //Turn off motors right_motor.Stop(); left_motor.Stop(); } void turn_left(int percent, int counts) //using encoders { //Reset encoder counts right_encoder.ResetCounts(); left_encoder.ResetCounts(); //Set both motors to desired percent right_motor.SetPercent(percent); left_motor.SetPercent(-percent); //While the average of the left and right encoder are less than counts, //keep running motors while((left_encoder.Counts() + right_encoder.Counts()) / 2. < counts); //Turn off motors right_motor.Stop(); left_motor.Stop(); } void check_x_plus(float x_coordinate) //using RPS while robot is in the +x direction { //check whether the robot is within an acceptable range while(RPS.X() < x_coordinate - 1 || RPS.X() > x_coordinate + 1) { if(RPS.X() > x_coordinate) { //pulse the motors for a short duration in the correct direction right_motor.SetPercent(10); left_motor.SetPercent(10); Sleep(10); right_motor.SetPercent(0); left_motor.SetPercent(0); } else if(RPS.X() < x_coordinate) { //pulse the motors for a short duration in the correct direction right_motor.SetPercent(-10); left_motor.SetPercent(-10); Sleep(10); right_motor.SetPercent(0); left_motor.SetPercent(0); } } } void check_y_minus(float y_coordinate) //using RPS while robot is in the -y direction { //check whether the robot is within an acceptable range while(RPS.Y() < y_coordinate - 1 || RPS.Y() > y_coordinate + 1) { if(RPS.Y() > y_coordinate) { //pulse the motors for a short duration in the correct direction right_motor.SetPercent(-10); left_motor.SetPercent(-10); Sleep(10); right_motor.SetPercent(0); left_motor.SetPercent(0); } else if(RPS.Y() < y_coordinate) { //pulse the motors for a short duration in the correct direction right_motor.SetPercent(10); left_motor.SetPercent(10); Sleep(10); right_motor.SetPercent(0); left_motor.SetPercent(0); } } } void check_y_plus(float y_coordinate) //using RPS while robot is in the +y direction { //check whether the robot is within an acceptable range while(RPS.Y() < y_coordinate - 1 || RPS.Y() > y_coordinate + 1) { if(RPS.Y() > y_coordinate) { //pulse the motors for a short duration in the correct direction right_motor.SetPercent(10); left_motor.SetPercent(10); Sleep(10); right_motor.SetPercent(0); left_motor.SetPercent(0); } else if(RPS.Y() < y_coordinate) { //pulse the motors for a short duration in the correct direction right_motor.SetPercent(-10); left_motor.SetPercent(-10); Sleep(10); right_motor.SetPercent(0); left_motor.SetPercent(0); } } } void check_heading(float heading) //using RPS { //you will need to fill out this one yourself and take into account //the edge conditions (when you want the robot to go to 0 degrees //or close to 0 degrees) while(RPS.Heading() < heading + 360*(heading==0) - 1 || RPS.Heading() > heading - 360*(heading==0) + 1 ) { LCD.WriteRC(RPS.Heading() ,5,5); if (RPS.Heading() < heading + 360*(heading==0) - 1 ) { left_motor.SetPercent(10); right_motor.SetPercent(-10); Sleep(10); left_motor.SetPercent(0); right_motor.SetPercent(0); } if (RPS.Heading() > heading) { left_motor.SetPercent(-10); right_motor.SetPercent(10); Sleep(15); left_motor.SetPercent(0); right_motor.SetPercent(0); } } } int main(void) { float touch_x,touch_y; RPS.InitializeTouchMenu(); SD.OpenLog(); LCD.WriteLine("RPS & Data Logging Test"); LCD.WriteLine("Press Screen To Start"); while(!LCD.Touch(&touch_x,&touch_y)); //Wait for touchscreen press SD.Printf("%f, %f\n", RPS.X(), RPS.Y()); move_forward(-30, 240); check_y_plus(19.5); SD.Printf("%f, %f\n", RPS.X(), RPS.Y()); turn_right(-30, 205); check_heading(270); move_forward(-30, 21 * 40 ); check_x_plus(28.8); SD.Printf("%f, %f\n", RPS.X(), RPS.Y()); turn_right(-30, 205); check_heading(180); move_forward(-30, 240); check_y_minus(11.7); SD.Printf("%f, %f\n", RPS.X(), RPS.Y()); check_heading(180); SD.CloseLog(); return 0; }
-