//# GTU-X30A-Strider-Heavy-Arm- //University of Technology Robotics Project for IEEE South East Conference #include <Servo.h>
//// // Declaration of Motor Controller PINs // //// #define MFR_A_PIN 11 // FRONT RIGHT MOTOR #define MFR_B_PIN 12 #define MFR_PWM_PIN 13 // PULSE WIDTH MODULATION PIN
#define MFL_A_PIN 5 //FRONT LEFT MOTOR #define MFL_B_PIN 6 #define MFL_PWM_PIN 7 // PULSE WIDTH MODULATION PIN
#define MBR_A_PIN 9 //BACK RIGHT MOTOR #define MBR_B_PIN 10 #define MBR_PWM_PIN 8 // PULSE WIDTH MODULATION PIN
#define MBL_A_PIN 3 //BACK LEFT MOTOR #define MBL_B_PIN 4 #define MBL_PWM_PIN 2 // PULSE WIDTH MODULATION PIN
//// // Declaration of the IRF PINs // //// #define IRL_PIN_s0 22 #define IRL_PIN_s1 23 #define IRL_PIN_s2 24 #define IRL_PIN_s3 25 #define IRL_PIN_s4 26 #define IRL_PIN_s5 27 #define IRL_PIN_s6 28 #define IRL_PIN_s7 29
//// // Declaration of the Servo PINs // //// // Right Arm #define R_ARM1_PIN 42 #define R_ARM2_PIN 44 #define R_ARM3_PIN 46 #define R_ARM4_PIN 48 #define R_ARM5_PIN 50 #define R_ARMC_PIN 52
// Left Arm #define L_ARM1_PIN 43 #define L_ARM2_PIN 45 #define L_ARM3_PIN 47 #define L_ARM4_PIN 49 #define L_ARM5_PIN 51 #define L_ARMC_PIN 53
//// // LDR PIN Declaration // ////
#define LDR_PIN 0
//// // Servo Object Declaration // ////
// Right Arm Servo rArm_Servo1; Servo rArm_Servo2; Servo rArm_Servo3; Servo rArm_Servo4; Servo rArm_Servo5; Servo rClaw;
// Left Arm Servo lArm_Servo1; Servo lArm_Servo2; Servo lArm_Servo3; Servo lArm_Servo4; Servo lArm_Servo5; Servo lClaw;
int LDR_value; int initial_condition; int State;
int IRL_in; // declaration of the buffer to store the output of all s0- s7 pins of the KRF int game_count = 0; int t_count = 0;
int xFR = 147; int xFL = 157; int xBR = 130; int xBL = 130;
void setup() { Serial.begin(9600); //Opens serial connection at 9600 baud rate
//Initaizing Robotic Right Arm Angles
rArm_Servo1.write(150);
rArm_Servo2.write(170);
rArm_Servo3.write(180);
rArm_Servo4.write(180);
rArm_Servo5.write(180);
//Initiaizing Robotic Left Arm Angles
lArm_Servo1.write(10);
lArm_Servo2.write(5);
lArm_Servo3.write(5);
lArm_Servo4.write(5);
lArm_Servo5.write(5);
//Initiaizng Heavy Arm Claw to hold position.
lClaw.writeMicroseconds(1500);
rArm_Servo1.attach(R_ARM1_PIN); // Attaches(inputs the pin out to the Servo object) class) the servos to their respective pin
rArm_Servo2.attach(R_ARM2_PIN); // Attaches(inputs the pin out to the Servo object) class) the servos to their respective pin
rArm_Servo3.attach(R_ARM3_PIN); // Attaches(inputs the pin out to the Servo object) class) the servos to their respective pin
rArm_Servo4.attach(R_ARM4_PIN); // Attaches(inputs the pin out to the Servo object) class) the servos to their respective pin
//rArm_Servo5.attach(R_ARM5_PIN); // Attaches(inputs the pin out to the Servo object) class) the servos to their respective pin
//rClaw.attach(R_ARMC_PIN); // Attaches(inputs the pin out to the Servo object) class) the servos to their respective pin
lArm_Servo1.attach(L_ARM1_PIN); // Attaches(inputs the pin out to the Servo object) class) the servos to their respective pin
lArm_Servo2.attach(L_ARM2_PIN); // Attaches(inputs the pin out to the Servo object) class) the servos to their respective pin
lArm_Servo3.attach(L_ARM3_PIN); // Attaches(inputs the pin out to the Servo object) class) the servos to their respective pin
lArm_Servo4.attach(L_ARM4_PIN); // Attaches(inputs the pin out to the Servo object) class) the servos to their respective pin
//lArm_Servo5.attach(L_ARM5_PIN); // Attaches(inputs the pin out to the Servo object) class) the servos to their respective pin
lClaw.attach(L_ARMC_PIN); // Attaches(inputs the pin out to the Servo object) class) the servos to their respective pin
delay(3000);
//LDR_value = analogRead(LDR_PIN); //reading sensor digital values
initial_condition = map(analogRead(LDR_PIN), 0, 1023, 0, 40);// map LDR reading to smaller values and store as initial_condition
//state = initial_condition; //place initial condition in variable state for use later on
Serial.print("Reading taken (initial) = ");
Serial.println(initial_condition);
delay(1000);
do
{
Serial.print("Reading taken = ");
State = map(analogRead(LDR_PIN), 0, 1023, 0, 40);
Serial.println(State);
}while((initial_condition - 2) >= State);
Serial.println("Ramp");
Set_Motors_Forward();
for(int i = 0; i < 20; i += 2)
{
analogWrite(MFR_PWM_PIN, xFR + i);
analogWrite(MFL_PWM_PIN, xFL + i);
analogWrite(MBR_PWM_PIN, xBR + i);
analogWrite(MBL_PWM_PIN, xBR + i);
}
delay(500);
}
void loop() { IRL_Read();
switch(IRL_in)
{
//Sensor conditions that indicate that the line is at the center
case 0b10000001:
case 0b11000011:
case 0b11100111:
Serial.println("00L00");
Forward_Motors();
break;
case 0b10000111:
Set_Motors_Forward();
analogWrite(MFR_PWM_PIN, xFR + 60);
analogWrite(MFL_PWM_PIN, xFL - 40);
analogWrite(MBR_PWM_PIN, xBR + 60);
analogWrite(MBL_PWM_PIN, xBL - 40);
break;
case 0b00001111:
Serial.println("0L000");
Set_Motors_Forward();
analogWrite(MFR_PWM_PIN, xFR + 60);
analogWrite(MFL_PWM_PIN, xFL - 60);
analogWrite(MBR_PWM_PIN, xBR + 60);
analogWrite(MBL_PWM_PIN, xBL - 60);
break;
case 0b11000111:
Set_Motors_Forward();
analogWrite(MFR_PWM_PIN, xFR + 60);
analogWrite(MFL_PWM_PIN, xFL - 30);
analogWrite(MBR_PWM_PIN, xBR + 60);
analogWrite(MBL_PWM_PIN, xBL - 30);
break;
case 0b10000011:
Serial.println("0LL00");
Set_Motors_Forward();
analogWrite(MFR_PWM_PIN, xFR + 40);
analogWrite(MFL_PWM_PIN, xFL - 10);
analogWrite(MBR_PWM_PIN, xBR + 40);
analogWrite(MBL_PWM_PIN, xBL - 10);
break;
case 0b11100001:
analogWrite(MFR_PWM_PIN, xFR - 40);
analogWrite(MFL_PWM_PIN, xFL + 60);
analogWrite(MBR_PWM_PIN, xBR - 40);
analogWrite(MBL_PWM_PIN, xBL + 60);
case 0b11110000:
Serial.println("00LL0");
Set_Motors_Forward();
analogWrite(MFR_PWM_PIN, xFR - 60);
analogWrite(MFL_PWM_PIN, xFL + 60);
analogWrite(MBR_PWM_PIN, xBR - 60);
analogWrite(MBL_PWM_PIN, xBL + 60);
break;
case 0b11100011:
Set_Motors_Forward();
analogWrite(MFR_PWM_PIN, xFR - 30);
analogWrite(MFL_PWM_PIN, xFL + 60);
analogWrite(MBR_PWM_PIN, xBR - 30);
analogWrite(MBL_PWM_PIN, xBL + 60);
break;
case 0b11000001:
Serial.println("000L0");
Set_Motors_Forward();
analogWrite(MFR_PWM_PIN, xFR - 10);
analogWrite(MFL_PWM_PIN, xFL + 40);
analogWrite(MBR_PWM_PIN, xBR - 10);
analogWrite(MBL_PWM_PIN, xBL + 40);
break;
case 0b00111111:
case 0b01111111:
Serial.println("L0000");
/*
Set_Motors_Right();
analogWrite(MFR_PWM_PIN, xFR);
analogWrite(MFL_PWM_PIN, xFL);
analogWrite(MBR_PWM_PIN, xBR);
analogWrite(MBL_PWM_PIN, xBL);
*/
Set_Motors_Forward();
analogWrite(MFR_PWM_PIN, 255);
analogWrite(MFL_PWM_PIN, 0);
analogWrite(MBR_PWM_PIN, 255);
analogWrite(MBL_PWM_PIN, 0);
break;
case 0b11111100:
case 0b11111110:
Serial.println("0000L");
/*
Set_Motor_Left();
analogWrite(MFR_PWM_PIN, xFR);
analogWrite(MFL_PWM_PIN, xFL);
analogWrite(MBR_PWM_PIN, xBR);
analogWrite(MBL_PWM_PIN, xBL);
*/
Set_Motors_Forward();
analogWrite(MFR_PWM_PIN, 0);
analogWrite(MFL_PWM_PIN, 255);
analogWrite(MBR_PWM_PIN, 0);
analogWrite(MBL_PWM_PIN, 255);
break;
case 0b11111111:
Serial.println("00000");
Stop_Motors();
break;
case 0b00000000:
Serial.println("LLLLL");
Stop_Motors();
if(t_count < game_count)
{
Right_Turn_Until_Line();
}else{
switch(game_count)
{
case 0:
game_count++;
break;
case 1:
game_count++;
break;
case 2:
game_count++;
break;
case 3:
game_count++;
break;
case 4:
Forward_Motors();
delay(500);
Stop_Motors();
while(1);
break;
}
Stop_Motors();
Forward_Motors();
delay(250);
Stop_Motors();
Turn_Left_Motors(20);
delay(500);
while(IRL_in != 0b11000011 && IRL_in != 0b11000111 && IRL_in != 0b10000011 && IRL_in != 0b00111111)
{
IRL_Read();
}
Serial.println("STOP AFTER WHILE");
Stop_Motors();
}
//while(1);
/*
Stop_Motors();
Forward_Motors();
delay(250);
Stop_Motors();
do
{
Turn_Left_Motors(30);
IRL_Read();
}while(IRL_in != 11000011 || IRL_in != 11000111 || IRL_in != 11100011);
Serial.println("STOP AFTER WHILE");
Stop_Motors();
*/
break;
case 0b00000111:
Set_Motors_Forward();
analogWrite(MFR_PWM_PIN, xFR + 60);
analogWrite(MFL_PWM_PIN, xFL - 80);
analogWrite(MBR_PWM_PIN, xBR + 60 );
analogWrite(MBL_PWM_PIN, xBL - 80);
break;
case 0b00000011:
case 0b00000001:
Serial.println("LLL00/LLLL0");
Left_Turn_Until_Line();
//while(1);
break;
case 0b11100000:
Set_Motors_Forward();
analogWrite(MFR_PWM_PIN, xFR - 80);
analogWrite(MFL_PWM_PIN, xFL + 60);
analogWrite(MBR_PWM_PIN, xBR - 80);
analogWrite(MBL_PWM_PIN, xBL + 60);
break;
case 0b11000000:
case 0b10000000:
Serial.println("LLL00/LLLL0");
//while(1);
break;
default:
Serial.println("Default Error");
break;
}
}
void Stop_Motors(void) { Serial.println("Stop"); analogWrite(MFR_PWM_PIN, 0); analogWrite(MFL_PWM_PIN, 0);
analogWrite(MBR_PWM_PIN, 0);
analogWrite(MBL_PWM_PIN, 0);
}
void IRL_Read(void) { IRL_in = 0;
for(int i = 0; i <= 7; i++)
{
IRL_in = (IRL_in << 1) + digitalRead(IRL_PIN_s7 - i);
}
Serial.println(IRL_in, BIN);
}
void Forward_Motors(void) { Serial.println("Forward");
Set_Motors_Forward();
analogWrite(MFR_PWM_PIN, xFR);
analogWrite(MFL_PWM_PIN, xFL);
analogWrite(MBR_PWM_PIN, xBR);
analogWrite(MBL_PWM_PIN, xBL);
}
void Forward_Motors(int i) { Serial.println("Forward+");
Set_Motors_Forward();
analogWrite(MFR_PWM_PIN, xFR + i);
analogWrite(MFL_PWM_PIN, xFL + i);
analogWrite(MBR_PWM_PIN, xBR + i);
analogWrite(MBL_PWM_PIN, xBL + i);
}
void Turn_Left_Motors(int i) { Serial.println("Left");
Set_Motors_Left();
analogWrite(MFR_PWM_PIN, xFR + i);
analogWrite(MFL_PWM_PIN, xFL + i);
analogWrite(MBR_PWM_PIN, xBR + i);
analogWrite(MBL_PWM_PIN, xBL + i);
}
void Turn_Right_Motors(int i) { Serial.println("Left");
Set_Motors_Right();
analogWrite(MFR_PWM_PIN, xFR + i);
analogWrite(MFL_PWM_PIN, xFL + i);
analogWrite(MBR_PWM_PIN, xBR + i);
analogWrite(MBL_PWM_PIN, xBL + i);
} void Set_Motors_Forward(void) { Serial.print("Set Motors for Forward"); digitalWrite(MFR_A_PIN, LOW); digitalWrite(MFR_B_PIN, HIGH);
digitalWrite(MFL_A_PIN, LOW);
digitalWrite(MFL_B_PIN, HIGH);
digitalWrite(MBR_A_PIN, HIGH);
digitalWrite(MBR_B_PIN, LOW);
digitalWrite(MBL_A_PIN, LOW);
digitalWrite(MBL_B_PIN, HIGH);
}
void Set_Motors_Left(void) { Serial.print("Set Motors for Left"); digitalWrite(MFR_A_PIN, LOW); digitalWrite(MFR_B_PIN, HIGH);
digitalWrite(MFL_A_PIN, HIGH);
digitalWrite(MFL_B_PIN, LOW);
digitalWrite(MBR_A_PIN, HIGH);
digitalWrite(MBR_B_PIN, LOW);
digitalWrite(MBL_A_PIN, HIGH);
digitalWrite(MBL_B_PIN, LOW);
}
void Set_Motors_Right(void) { Serial.print("Set Motors for Right"); digitalWrite(MFR_A_PIN, HIGH); digitalWrite(MFR_B_PIN, LOW);
digitalWrite(MFL_A_PIN, LOW);
digitalWrite(MFL_B_PIN, HIGH);
digitalWrite(MBR_A_PIN, LOW);
digitalWrite(MBR_B_PIN, HIGH);
digitalWrite(MBL_A_PIN, LOW);
digitalWrite(MBL_B_PIN, HIGH);
}
void Left_Turn_Until_Line(void) { Stop_Motors(); Forward_Motors();
delay(250);
Stop_Motors();
Turn_Left_Motors(20);
delay(250);
while(IRL_in != 0b11000011 && IRL_in != 0b11000111 && IRL_in != 0b10000011 && IRL_in != 0b00111111)
{
IRL_Read();
}
Serial.println("STOP AFTER WHILE");
Stop_Motors();
}
void Right_Turn_Until_Line(void) { Stop_Motors(); Forward_Motors();
delay(250);
Stop_Motors();
Turn_Right_Motors(20);
while(IRL_in != 0b11100011 && IRL_in != 0b11000011 && IRL_in != 0b1111100 && IRL_in != 0b11100001)
{
IRL_Read();
}
Serial.println("STOP AFTER WHILE");
Stop_Motors();
}