 	for (int id=1; id<6; id++) {
 		Dxl.jointMode(id);
 		Dxl.ledOn(id);
-	} 
+	}
 
 	Dxl.writeWord(BROADCAST_ID,11,100);
 
 	Dxl.goalPosition(1, START_1);
 	Dxl.goalPosition(2, START_2);
 	Dxl.goalPosition(3, START_3);
-	
+
 	for (int i=0; i<5; i++) {
 		gripper(i%2);
 		delay(500);

			Dxl.maxTorque(id,val);
			break;
		} case 'g': { // general write command
			int add; 
			if (!readInt(&add)) break; 
			Dxl.writeWord(id, add, val);
			break;
		
			// examples of using writeWord()
			//Dxl.writeWord(id, 30, val); //move to Goal Position
			//Dxl.writeWord(id, 32, val); //move at Speed

			break;
		} case 'v': {
			digitalWrite(SONAR_TRIGGER, HIGH);
  			delay(1);
			digitalWrite(SONAR_TRIGGER, LOW);

  			int sonarOne = analogRead(SONAR_LEFT);
    		int sonarTwo = analogRead(SONAR_RIGHT);

  			Serial2.println(sonarOne);
  			Serial2.println(sonarTwo);
			
			break;
		}
	}
