void setup()
{
shoulder.attach(SHOULDER_PIN); // Connect shoulder servo object
elbow.attach(ELBOW_PIN); // connect elbow servo object
wrist.attach(WRIST_PIN); // Connect elbow servo object
pinMode(BASEA, OUTPUT); // Drive motor on base is output
pinMode(BASEB, OUTPUT); // Other pin of drive motor on base
pinMode(PUMPA, OUTPUT); // Pump is driven by output
pinMode(PUMPB, OUTPUT); // Other pin of pump
pinMode(VENTPIN, OUTPUT); // Vent solenoid is single ended output
pinMode(ENCODER_PIN, INPUT); // Input for encoder disk on base
pinMode(LEFT_LIMIT_PIN, INPUT); // Input of limit switch on base (home)
pinMode(ACTUATOR_LIMIT, INPUT); // Actuator limit switch to detect disks
digitalWrite(LEFT_LIMIT_PIN, HIGH); // Turn on pullup
digitalWrite(ACTUATOR_LIMIT, HIGH); // Turn on pullup
attachInterrupt(0, encoder_debounce, CHANGE); // Trigger interrupt on state change
attachInterrupt(1, limit_debounce, FALLING); // trigger interrupt on low
Serial.begin(9600); // 9600 baud, 8N1
Serial.println("ready\n"); // Notify host that arm is ready for command
lift_wrist(WRIST_RAISED_POSITION); // Lift the wrist to top position
arm_up(); // Lift the arm to the top position
delay(500); // Give the servos time to settle
if (digitalRead(LEFT_LIMIT_PIN) == LOW) current_location = 0;
left_home(); // Move the arm to the home position
if (echo) prompt(); // If the echo is on then print the prompt
}
void setup(void) {
//PlayMelody();
Serial.begin(115200); // Serialle Schnittstelle mit 115200 Baud
myservo1.attach(4);
myservo2.attach(5);
}
void setup() {
Serial.begin(9600);
calibrateAll();
leftServo.attach(9, 1300, 1700);
rightServo.attach(10, 1300, 1700);
while(!finishLine){
driveStraight(10, 50);
readColor();
}
turnLeftInDegrees(90);
driveInInches(36);
for(int i = 0; i < 12; i++){
turnRightInDegrees(90);
driveInInches(3);
turnRightInDegrees(90);
driveInInches(72);
turnLeftInDegrees(90);
driveInInches(3);
turnLeftInDegrees(90);
driveInInches(72);
}
}
void initMotors()
{
leftMotor.attach(LMPWMPin);
rightMotor.attach(RMPWMPin);
leftMotor.writeMicroseconds(1500); // set servo to mid-point
rightMotor.writeMicroseconds(1500);
}
/* indicate pin numbers for each servo */
void JgBigFootRobot::init(int pinFootRight, int pinHipRight, int pinFootLeft, int pinHipLeft, int pinEyes) {
servoFootRight.attach(pinFootRight);
servoHipRight.attach(pinHipRight);
servoFootLeft.attach(pinFootLeft);
servoHipLeft.attach(pinHipLeft);
servoEyes.attach(pinEyes);
}
void attach() {
Serial2.println("attaching");
servo1.attach(SERVO_PIN1);
servo2.attach(SERVO_PIN2);
ASSERT(servo1.attachedPin() == SERVO_PIN1);
ASSERT(servo2.attachedPin() == SERVO_PIN2);
}
void loop() {
int digit = counter;
int dig1 = digit % 10;
digit /= 10;
int dig2 = digit % 10;
digit /= 10;
int dig3 = digit % 10;
digit /= 10;
int dig4 = digit % 10;
digit /= 10;
int dig5 = digit % 10;
lc.clearDisplay(0);
//lc.setDigit(0,7,0,false);
//lc.setDigit(0,6,0,false);
//lc.setDigit(0,5,0,false);
lc.setDigit(0,4,dig5,false);
lc.setDigit(0,3,dig4,false);
lc.setDigit(0,2,dig3,false);
lc.setDigit(0,1,dig2,false);
lc.setDigit(0,0,dig1,false);
// up
if(state == 0) {
if(counter > 0) {
counter --;
} else {
pos=100;
myservo.attach(servopin);
myservo.write(160);
delay(200);
myservo.write(140);
delay(200);
myservo.write(120);
delay(200);
myservo.write(pos);
delay(200);
myservo.detach();
state = 1;
counter = low;
}
// down
} else if(state ==1) {
if(counter > 0) {
counter --;
} else {
pos=180;
myservo.attach(servopin);
myservo.write(pos);
delay(500);
myservo.detach();
state = 0;
counter = high;
}
}
delay(delaytime);
}
void setup()
{
Serial.begin(9600);
servo1.attach(9); // attaches the servo on pin 9 to the servo object
servo2.attach(10);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
}
void SBRServoBegin(int pin1, int pin2, unsigned long curTime)
{
servo1.attach(pin1); // attaches the servo
servo2.attach(pin2);
spdTime = curTime;
degTime = curTime;
}
void setup() {
left.attach(2,1300,1700);
right.attach(13,1300,1700);
// attaches the servo on pin 9 to the servo object
left.write(90);
right.write(90);
turn(90);
clearMotors();
}
void initialize()
{
//ATTACH PAN AND TILT SERVOS
panServo.attach(panPin);
tiltServo.attach(tiltPin);
//INITIALIZE SERIAL @ 9600
Serial.begin(9600);
}
void setupMotors() {
bLM.attach(20);
bRM.attach(21);
tLM.attach(22);
tRM.attach(23);
writeToAll(MAX_THROTLE);
delay(5000);
writeToAll(MIN_THROTLE);
}
void setup() {
// Initialize WiServer and have it use the sendPage function to serve pages
Serial.begin(115200);
Serial.println("setup() ... ");
servoX.attach(2); // attaches the servo on pin 2 to the servo object
servoY.attach(4); // attaches the servo on pin 4 to the servo object
servoX.write(median);
servoY.write(median);
WiServer.init(sendPage);
}
void servos_setup()
{
// Set pin modes
pinMode(PIN_SERVO_RUDDER, OUTPUT);
pinMode(PIN_SERVO_SAIL, OUTPUT);
// Pin-object attachments
rudder_servo.attach(PIN_SERVO_RUDDER);
sail_servo.attach(PIN_SERVO_SAIL);
}
Context::Context(Commander *commander, DCServo *servo,
SpeedSensor *left, SpeedSensor *right,
int lPwm, int rPwm,
int lRev, int rRev,
PIDController *lSp, PIDController *rSp,
PIDController *pos,
ros::NodeHandle *nh,
JetsonCommander *jcommander,
geometry_msgs::Vector3 *odomsg,
ros::Publisher *pub,
geometry_msgs::Vector3 *commsg,
ros::Publisher *compub,
std_msgs::Float32 *angmsg,
ros::Publisher *angpub,
std_msgs::Float32 *angcommsg,
ros::Publisher *angcompub) {
_commander = commander;
_servo = servo;
_left = left;
_right = right;
_lPwm = lPwm;
_rPwm = rPwm;
_lRev = lRev;
_rRev = rRev;
_lSp = lSp;
_rSp = rSp;
_pos = pos;
_nh = nh; //$ ROS node handle
_jcommander = jcommander; //$ Jetson commander
//$ ROS publishers and messages
_odomsg = odomsg;
_pub = pub;
_commsg = commsg;
_compub = compub;
_angmsg = angmsg;
_angpub = angpub;
_angcommsg = angcommsg;
_angcompub = angcompub;
pinMode(_lPwm, OUTPUT);
pinMode(_rPwm, OUTPUT);
//pinMode(_lRev, OUTPUT);
//pinMode(_rRev, OUTPUT);
//digitalWrite(_lRev, HIGH);
//digitalWrite(_rRev, HIGH);
//ROBOCLAW
leftMotor.attach(_lPwm);
rightMotor.attach(_rPwm);
}
void setup(){
Serial.begin(9600);
calibrateAll();
leftServo.attach(9, 1300, 1700);
rightServo.attach(10, 1300, 1700);
while(!finishLine){
driveStraight(10, 50);
readColor();
}
turnLeftInDegrees(90);
driveInInches(36);
for(int i = 0; i < 12; i++){
turnRightInDegrees(90);
for(int i = 0; i < 30; i++){
if(reading >= yellowMin && reading <= yellowMax){
foundGold = true;
}
if(!foundGold){
driveInInches(0.1);
}
}
turnRightInDegrees(90);
for(int i = 0; i < 720; i++){
if(reading >= yellowMin && reading <= yellowMax){
foundGold = true;
}
if(!foundGold){
driveInInches(0.1);
}
}
turnLeftInDegrees(90);
for(int i = 0; i < 30; i++){
if(reading >= yellowMin && reading <= yellowMax){
foundGold = true;
}
if(!foundGold){
driveInInches(0.1);
}
}
turnLeftInDegrees(90);
for(int i = 0; i < 720; i++){
if(reading >= yellowMin && reading <= yellowMax){
foundGold = true;
}
if(!foundGold){
driveInInches(0.1);
}
}
}
}
void setup()
{
pinMode(INCREASE_ENGINES_PIN, INPUT);
pinMode(DECREASE_ENGINES_PIN, INPUT);
pinMode(LEFT_ENGINE_PIN, OUTPUT);
pinMode(RIGHT_ENGINE_PIN, OUTPUT);
_leftEngine.attach(LEFT_ENGINE_PIN);
_rightEngine.attach(RIGHT_ENGINE_PIN);
RTB* myRTB = 0;
while(true)
{
if(digitalRead(BUTTON_MODE_PIN) == 1)
{
if(myRTB == 0)
myRTB = new RTB();
else
{
delete myRTB;
myRTB = 0;
}
}
if(myRTB != 0)
{
if(digitalRead(DECREASE_ENGINES_PIN) == 1)
{
if(_currentSpeed <= 1496.251f)
{
_currentSpeed += 3.749f;
_leftEngine.write(_currentSpeed);
_rightEngine.write(_currentSpeed);
}
}
if(digitalRead(INCREASE_ENGINES_PIN) == 1)
{
if(_currentSpeed >= 547.754f)
{
_currentSpeed -= 3.749f;
_leftEngine.write(_currentSpeed);
_rightEngine.write(_currentSpeed);
}
}
}
else
myRTB->sync();
}
delete myRTB;
}
void setup()
{
// servo
servo1.attach(3);
servo2.attach(6);
servo3.attach(9);
pinMode(13, OUTPUT);
Serial.begin(9600);
MsTimer2::set(1000, setMode);
MsTimer2::start();
mode = STOP;
}
void setup() {
pinMode(POT_PIN, INPUT_ANALOG);
pinMode(BOARD_BUTTON_PIN, INPUT);
pinMode(BOARD_LED_PIN, OUTPUT);
Serial2.begin(9600);
servo1.attach(SERVO_PIN1, MIN_PW, MAX_PW, MIN_ANGLE1, MAX_ANGLE1);
servo2.attach(SERVO_PIN2, MIN_PW, MAX_PW, MIN_ANGLE2, MAX_ANGLE2);
ASSERT(servo1.attachedPin() == SERVO_PIN1);
ASSERT(servo2.attachedPin() == SERVO_PIN2);
}
void motorsInit( int leftPin, int rightPin )
{
servoLeft.attach(leftPin);
servoRight.attach(rightPin);
servoLeft.writeMicroseconds(servoSpeedLeft);
servoRight.writeMicroseconds(servoSpeedRight);
#ifdef USE_PID
Input = 0;
Setpoint = 0;
turningPID.SetMode(AUTOMATIC);
turningPID.SetOutputLimits(-SERVO_DRIVE_TURN_SPEED, SERVO_DRIVE_TURN_SPEED);
#endif
}
void setup() {
leftservo.attach(D0);
Particle.function("setposL", setPosL);
// Particle.variable("getposR", &posL, INT);
rightservo.attach(D1);
Particle.function("setposR", setPosR);
// Particle.variable("getposL", &posR, INT);
Particle.function("setpos", setPos);
}
void ServoFirmata::attach(byte pin, int minPulse, int maxPulse)
{
Servo *servo = servos[PIN_TO_SERVO(pin)];
if (!servo) {
servo = new Servo();
servos[PIN_TO_SERVO(pin)] = servo;
}
if (servo->attached())
servo->detach();
if (minPulse>=0 || maxPulse>=0)
servo->attach(PIN_TO_DIGITAL(pin),minPulse,maxPulse);
else
servo->attach(PIN_TO_DIGITAL(pin));
}
void setup() {
Serial.begin(9600);
esc.attach(ESC_PIN);
esc2.attach(ESC_PIN2);
esc3.attach(ESC_PIN3);
esc4.attach(ESC_PIN4);
int error;
uint8_t c;
Wire.begin();
error = MPU6050_read(MPU6050_WHO_AM_I, &c, 1);
error = MPU6050_read(MPU6050_PWR_MGMT_1, &c, 1);
MPU6050_write_reg(MPU6050_PWR_MGMT_1, 0);
}
// Perform initial setup of drivers.
void driver_setup()
{
// Attach servo controllers to ports.
servo_sensor.attach(PORT_S2);
magnet.attach(PORT_S1);
servo_speed.attach(PORT_S11, 1000, 2000);
servo_steer.attach(PORT_S12, 1000, 2000);
// Configure the smart servos.
Herkulex.beginSerial2(115200); //open serial port 1
Herkulex.reboot(10); //reboot first motor
Herkulex.reboot(20); //reboot first motor
delay(500);
Herkulex.initialize(); //initialize motors
}
int Thruster_Init()
{
Serial.println("Initialize Thrusters ...");
motor1.attach(MOTOR1_PIN);
motor2.attach(MOTOR2_PIN);
motor3.attach(MOTOR3_PIN);
motor4.attach(MOTOR4_PIN);
motor1.writeMicroseconds(MID_SIGNAL);
motor2.writeMicroseconds(MID_SIGNAL);
motor3.writeMicroseconds(MID_SIGNAL);
motor4.writeMicroseconds(MID_SIGNAL);
delay(10000);
return 1;
}
void init()
{
controller.attach(pin);
goal = 1500;
cur = goal;
controller.writeMicroseconds(cur);
}
void setup() {
pinMode(A0, INPUT);
pinMode(D0, OUTPUT);
Serial.begin(9600);
tempTimer.start();
myServo.attach(D0);
}
void setup() {
Serial.begin(9600);
while (!Serial) {
;
}
MCUSR = 0; // for Reset : clear out any flags of prior resets.
/*
if(!isDebug){//TODO Servo bug fix....
// Initialize SD card.
Serial.print(F("\nInitializing SD card..."));
if (card.init(SPI_HALF_SPEED, chipSelectPin)) {
//Serial.print(F("OK"));
} else {
//Serial.print(F("NG"));
abort();
}
memset(buffer, 0, 0x200);
}
*/
//For Neopixel
strip.begin();
strip.show(); // Initialize all pixels to 'off'
sv.attach(SERVO_PIN, 800, 2300);// For Servo
pinMode(buttonPin, INPUT);// for BUtton
}
void mode_test(){
SERIAL_OUT.println();
SERIAL_OUT.println();
SERIAL_OUT.println("toggle mode (i.e. TX CH3) and this function will print its current state");
SERIAL_OUT.println("perform hard reset to exit function");
esc.detach();
pinMode(THROTTLE, OUTPUT);
digitalWrite(THROTTLE, LOW);
bool attached = true;
long time_temp = 0;
while(1){
get_mode();
if(mode == MANUAL){
if(attached == true){
esc.detach();
// pinMode(THROTTLE, OUTPUT);
// digitalWrite(THROTTLE, LOW);
attached = false;
}
}
else if(mode == AUTOMATIC){
if(attached == false){
delay(250);
esc.attach(THROTTLE);
delay(250);
esc.writeMicroseconds(S_STOP);
attached = true;
}
}
else if(mode == AUX){
if(attached == true){
esc.detach();
// pinMode(THROTTLE, OUTPUT);
// digitalWrite(THROTTLE, LOW);
attached = false;
}
}
else if(mode == WP_MODE){
if(attached == true){
esc.detach();
// pinMode(THROTTLE, OUTPUT);
// digitalWrite(THROTTLE, LOW);
attached = false;
}
}
else SERIAL_OUT.println("nothing valid detected");
if((millis() - time_temp) > 250){
SERIAL_OUT.println(mode);
time_temp = millis();
}
}
return ;
}
void setup()
{
Serial.begin(9600);
Serial.setTimeout(100);
pinMode(pin_motor_in, INPUT_PULLUP);
pinMode(pin_servo_in, INPUT_PULLUP);
pinMode(pin_button_in, INPUT_PULLUP);
attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pin_motor_in), motor_interrupt, CHANGE);
attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pin_servo_in), servo_interrupt, CHANGE);
attachPinChangeInterrupt(digitalPinToPinChangeInterrupt(pin_button_in), button_interrupt, CHANGE);
servo.attach(pin_servo_out);
motor.attach(pin_motor_out);
}
