<p>// Router! int flexSensorPin1 = A0; int flexSensorPin2 = A1; int flexSensorPin3 = A2; int flexSensorPin4 = A3; int ledPin=13; int button=8; int button_read=0; int b=0; int k=0; char s_str1[2]; char s_str2[2]; char s_str3[2]; char s_str4[2]; char command='e'; String str1,str2,str3,str4; void setup(){ pinMode(button,INPUT_PULLUP); // The button is going to control if we are going to control either the robot arm or the motors. Serial.begin(9600); } void loop(){</p><p> if (digitalRead(button) == LOW) // check if button was pressed { b++; b=b%2; delay(250); if (b==0){ command='s'; digitalWrite(ledPin,HIGH); // indicator on the hand to know if the motors is going to be controlled or the robot arm } else { command='e'; digitalWrite(ledPin,LOW); } }</p><p>// Read the values int flexS1 = analogRead(flexSensorPin1); int flexS2 = analogRead(flexSensorPin2); int flexS3 = analogRead(flexSensorPin3); int flexS4 = analogRead(flexSensorPin4);</p><p>// Numbers below is individual for each of the flex sensors if (flexS1 < 475) flexS1 = 475; else if (flexS1 > 710) flexS1 = 710; if (flexS2 < 450) flexS2 = 450; else if (flexS2 > 700) flexS2 = 700; if (flexS3 < 450) flexS3 = 450; else if (flexS3 > 700) flexS3 = 700; if (flexS4 < 300) flexS4 = 300; else if (flexS4 > 455) flexS4 = 455;</p><p>// Mapping all the sensor values down between 0-9. (0 = full bent, 9 = unbent) int flex_1_0to100 = map(flexS1, 710, 475, 9, 0); int flex_2_0to100 = map(flexS2, 700, 450, 9, 0); int flex_3_0to100 = map(flexS3, 700, 450, 9, 0); int flex_4_0to100 = map(flexS4, 455, 300, 9, 0);</p><p>// Converting all strings to a single char str1=String(flex_1_0to100); str1.toCharArray(s_str1,2);</p><p> str2=String(flex_2_0to100); str2.toCharArray(s_str2,2);</p><p> str3=String(flex_3_0to100); str3.toCharArray(s_str3,2);</p><p> str4=String(flex_4_0to100); str4.toCharArray(s_str4,2);</p><p> if (command=='e'){ // Send all values to coordinator Serial.print("e"); // Indicates the start of the message (e=motors, s=robot arm) Serial.print(s_str4); // Thumb Serial.print(s_str2); // Index-finger Serial.print(s_str3); // Middle-finger Serial.print(s_str1); // Pinky</p><p> delay(50); } else if (command=='s'){ //Send all values to coordinator Serial.print("s"); // Indicates the start of the message (e=motors, s=robot arm) Serial.print(s_str4); // Thumb Serial.print(s_str2); // Index finger Serial.print(s_str3); // Middle finger Serial.print(s_str1); // Pinky delay(50); </p><p>} }</p><p>// Coordinator! #include <Servo.h> #define rightMotor1 8 #define rightMotor2 7 #define leftMotor1 9 #define leftMotor2 10 int claw_ang=0; int tilt_ang=15; Servo claw; Servo tilt; void setup() { claw.attach(5); tilt.attach(6); pinMode(rightMotor1, OUTPUT); pinMode(rightMotor2, OUTPUT); pinMode(leftMotor1, OUTPUT); pinMode(leftMotor2, OUTPUT); Serial.begin(9600); } void loop() { if (Serial.available() >=5 ) { char incomingByte1 = Serial.read(); char incomingByte2 = Serial.read(); char incomingByte3 = Serial.read(); char incomingByte4 = Serial.read(); char incomingByte5 = Serial.read();</p><p> if(incomingByte1=='e'){ // if the first byte is an 'e' then we going to control the motors int val_1 = incomingByte2-'0'; int val_2 = incomingByte3-'0'; int val_3 = incomingByte4-'0'; int val_4 = incomingByte5-'0';</p><p>// For debugging Serial.print("incomingByte1 = "); Serial.print(incomingByte1); Serial.print(" val_1 = "); Serial.print(val_1); Serial.print(" val_2 = "); Serial.print(val_2); Serial.print(" val_3 = "); Serial.print(val_3); Serial.print(" val_4 = "); Serial.println(val_4);</p><p> delay (250);</p><p>//Drive forwards if(val_1==0 && val_2==0 && val_3==0 && val_4==0 ){ digitalWrite(rightMotor1, LOW); digitalWrite(rightMotor2, HIGH); digitalWrite(leftMotor2, HIGH); digitalWrite(leftMotor1, LOW); } //Drive backwards else if(val_1==0 && val_2>0 && val_3>0 && val_4>0){ digitalWrite(rightMotor1, HIGH); digitalWrite(rightMotor2, LOW); digitalWrite(leftMotor2, LOW); digitalWrite(leftMotor1, HIGH); } //Drive right else if(val_1==0 && val_2>0 && val_3>0 && val_4==0){ digitalWrite(rightMotor1, LOW); digitalWrite(rightMotor2, HIGH); digitalWrite(leftMotor2, LOW); digitalWrite(leftMotor1, LOW); } //Drive left else if(val_1==0 && val_2==0 && val_3>0 && val_4==0){ digitalWrite(rightMotor1, LOW); digitalWrite(rightMotor2, LOW); digitalWrite(leftMotor2, HIGH); digitalWrite(leftMotor1, LOW); }</p><p> else if (val_1>0 && val_2>0 && val_3>0 && val_4>0){ digitalWrite(rightMotor1, LOW); digitalWrite(rightMotor2, LOW); digitalWrite(leftMotor2, LOW); digitalWrite(leftMotor1, LOW); }</p><p>}</p><p> else if (incomingByte1=='s'){ // if the first byte is 's' then we will control the robot arm int val_1 = incomingByte2-'0'; // val_3 corresponds to the thumb int val_2 = incomingByte3-'0'; // val_4 corresponds to index finger int val_3 = incomingByte4-'0'; // val_5 corresponds to the middle finger int val_4 = incomingByte5-'0'; // val_6 corresponds to the pinky</p><p>//For debugging /* Serial.println("Robot arm mode"); Serial.println(val_1); Serial.println(val_2); Serial.println(val_3); Serial.println(val_4); Serial.println();*/</p><p>// Close claw if(val_1==0 && val_2==0 && val_3==0 && val_4==0){ claw_ang-=5; } // Open claw else if (val_1==0 && val_2>0 && val_3>0 && val_4>0){ claw_ang+=5; } // Tilt forward else if (val_1==0 && val_2>0 && val_3>0 && val_4==0){ tilt_ang+=10; } //Tilt backward else if (val_1==0 && val_2>0 && val_3==0 && val_4==0){ tilt_ang-=10; }</p><p> if(claw_ang<0){ claw_ang=0; } if (claw_ang>145){ claw_ang=145; } if (tilt_ang<15){ tilt_ang=15; } if(tilt_ang>180){ tilt_ang=180; }</p><p>// For debugging Serial.println("Claw angle:"); Serial.println(claw_ang); Serial.println(); Serial.println("Tilt angle:"); Serial.println(tilt_ang); Serial.println();</p><p> claw.write(claw_ang); delay(10); tilt.write(tilt_ang); delay(10); </p><p>} delay(100); } }</p>