Professional Documents
Culture Documents
// initialize motors, this is to assign the enable pins, input and output pins of each
// IC. This means to allow the MCU to pass/send electric signals to each IC
// Pins.
int motorPin1 = 4; //
int motorPin2 = 3; // motor 1 (Gripper)
int enablePin1 = 14; //
int motorPin3 = 5; //
int motorPin4 = 6; // motor 2 (Wrist)
int enablePin2 = 7; //
int motorPin5 = 8; //
int motorPin6 = 9; // motor 3 (Elbow)
int enablePin3 = 10; //
// Start of code
void setup() {
// Set up all the motor pins connected to Arduino, this will allow the each pins to
// recognize by the Arduino while uploading the Arduino code.
// motor 1
pinMode(motorPin1, OUTPUT);
pinMode(motorPin2, OUTPUT);
pinMode(enablePin1, OUTPUT);
digitalWrite(enablePin1, HIGH);
// motor 2
pinMode(motorPin3, OUTPUT);
pinMode(motorPin4, OUTPUT);
pinMode(enablePin2, OUTPUT);
digitalWrite(enablePin2, HIGH);
// motor 3
pinMode(motorPin5, OUTPUT);
pinMode(motorPin6, OUTPUT);
pinMode(enablePin3, OUTPUT);
digitalWrite(enablePin3, HIGH);
// motor 4
pinMode(motorPin7, OUTPUT);
pinMode(motorPin8, OUTPUT);
pinMode(enablePin4, OUTPUT);
digitalWrite(enablePin4, HIGH);
// motor 5
pinMode(motorPin9, OUTPUT);
pinMode(motorPin10, OUTPUT);
pinMode(enablePin5, OUTPUT);
digitalWrite(enablePin5, HIGH);
// motor 6
pinMode(motorPin11, OUTPUT);
pinMode(motorPin12, OUTPUT);
pinMode(enablePin6, OUTPUT);
digitalWrite(enablePin6, HIGH);
// motor 7
pinMode(motorPin13, OUTPUT);
pinMode(motorPin14, OUTPUT);
pinMode(enablePin7, OUTPUT);
digitalWrite(enablePin7, HIGH);
Serial.begin(9600);
}
// if statement, this code determines the signal/data that is sent by the Android
// Device which have an equivalent action in Arduino MCU.
// Setting up and checking Bluetooth Connection.
void loop(){
// Light_On_Off
if ( state == 'X'){
light_on();
}
else if (state == 'Y'){
light_off();
}
// Clip_Close_Open
if (state == '1'){
clip_close();
}
else if(state == '2'){
clip_open();
}
// Wrist_Up_Down
else if(state == '3'){
wrist_up();
}
else if(state == '4'){
wrist_down();
}
//Elbow_Up_Down
else if(state == '7'){
elbow_up();
}
else if(state == '8'){
elbow_down();
}
//Shoulder_Up_Down
else if(state == '5'){
shoulder_up();
}
else if(state == '6'){
shoulder_down();
}
// Rotation_C_CW
else if(state == 'C'){
clockwise();
}
else if(state == 'W'){
cclockwise();
}
// Void function,
// the void function organize the code which is the key to minimize the error in
// debugging the Arduino code. In addition, each void function has an equivalent
// task in sending different mode of signals (high and low mode) in each IC pins.
}
// clip
void clip_close(){
digitalWrite(motorPin1, LOW);
digitalWrite(motorPin2, HIGH);
if (flag == 0){
Serial.println("Motor: Close");
flag = 1;
}
}
void clip_open(){
digitalWrite(motorPin1, HIGH);
digitalWrite(motorPin2, LOW);
if (flag == 0){
Serial.println("Motor: Open");
flag = 1;
}
}
// wrist
void wrist_up(){
digitalWrite(motorPin3, LOW);
digitalWrite(motorPin4, HIGH);
if (flag == 0){
Serial.println("Motor: Up");
flag = 1;
}
}
void wrist_down(){
digitalWrite(motorPin3, HIGH);
digitalWrite(motorPin4, LOW);
if (flag == 0){
Serial.println("Motor: Down");
flag = 1;
}
}
// shoulder
void shoulder_up(){
digitalWrite(motorPin5, LOW);
digitalWrite(motorPin6, HIGH);
if (flag == 0){
Serial.println("Motor: Up");
flag = 1;
}
}
void shoulder_down(){
digitalWrite(motorPin5, HIGH);
digitalWrite(motorPin6, LOW);
if (flag == 0){
Serial.println("Motor: Down");
flag = 1;
}
}
// elbow
void elbow_up(){
digitalWrite(motorPin7, LOW);
digitalWrite(motorPin8, HIGH);
if (flag == 0){
Serial.println("Motor: Up");
flag = 1;
}
}
void elbow_down(){
digitalWrite(motorPin7, HIGH);
digitalWrite(motorPin8, LOW);
if (flag == 0){
Serial.println("Motor: Down");
flag = 1;
}
void elbow_down(){
digitalWrite(motorPin7, HIGH);
digitalWrite(motorPin8, LOW);
if (flag == 0){
Serial.println("Motor: Down");
flag = 1;
}
}
// Rotation_Clock_Counter Drive
void clockwise(){
digitalWrite(motorPin9, HIGH);
digitalWrite(motorPin10, LOW);
if (flag == 0){
Serial.println("Motor: Clockwise");
flag = 1;
}
}
void cclockwise(){
digitalWrite(motorPin9, LOW);
digitalWrite(motorPin10, HIGH);
if (flag == 0){
Serial.println("Motor: CounterClockwise");
flag = 1;
}
}