void loop() {
delay(250);
toggleLED();
if (isButtonPressed()) {
if (servo1.attached()) detach();
else attach();
}
if (!servo1.attached()) return;
int32 average = averageAnalogReads(250);
int16 angle1 = (int16)map(average, 0, 4095, MIN_ANGLE1, MAX_ANGLE1);
int16 angle2 = (int16)map(average, 0, 4095, MIN_ANGLE2, MAX_ANGLE2);
print_buf("pot reading = %d, angle 1 = %d, angle 2 = %d.",
average, angle1, angle2);
servo1.write(angle1);
servo2.write(angle2);
int16 read1 = servo1.read();
int16 read2 = servo2.read();
print_buf("write/read angle 1: %d/%d, angle 2: %d/%d",
angle1, read1, angle2, read2);
ASSERT(abs(angle1 - read1) <= 1);
ASSERT(abs(angle2 - read2) <= 1);
print_buf("pulse width 1: %d, pulse width 2: %d",
servo1.readMicroseconds(), servo2.readMicroseconds());
Serial2.println("\n--------------------------\n");
}
void readColor(){
int reading = analogRead(A0);
Serial.println(reading);
if(reading >= blackMin && reading <= blackMax){
driveInInches(-10);
turnInDegrees(-90);
}
else if(reading >= blueMin && reading <= blueMax){
turnInDegrees(-45);
}
else if(reading >= greenMin && reading <= greenMax){
turnInDegrees(45);
}
else if(reading >= greyMin && reading <= greyMax){
driveInInches(8);
turnInDegrees(720);
}
else if(reading >= orangeMin && reading <= orangeMax){
leftServo.write(94);
rightServo.write(0);
delay(4800);
stopServos();
}
else if(reading >= whiteMin && reading <= whiteMax){
finishLine = true;
}
}
void loop() {
pressureV = analogRead(pressurepin); // read the pressure (0V to 5V mapped over 0 - 1024)
strainV = analogRead(strainpin); // read the strain (0V to 5V mapped over 0 - 1024)
refV = analogRead(refpin);
restartvalue = digitalRead(restartbutton);
startvalue = digitalRead(startbutton);
pressure = (float) (pressureV+0.095*refV)/(refV*0.009); // calculate correct pressure
if(startvalue== HIGH){
teststart = 1;
starttime = millis();
digitalWrite(pumppin, HIGH); // start pumping
}
if(restartvalue== HIGH || done == 1){
if (restartlastvalue == LOW){
teststart = 0; // stop testing
digitalWrite(pumppin, LOW); // stop pumping
myservo.write(minangle);
angle = minangle;
if(done == 0){
Serial.print('[');
Serial.print((float) millis()-starttime); Serial.print(' '); // print time since test started
Serial.print(strainV); Serial.print(' '); // print strain
Serial.print(pressure); Serial.print(' '); // print pressure
Serial.print(myservo.read()); Serial.print(' '); // print serial position
Serial.print(1);
Serial.println(']'); // Terminator statement
}
done = 0;
}
}
restartlastvalue = restartvalue;
// things to do when test is active
if (teststart == 1){
// when the air pressure is below the trigger pressure, start pulling
if(pressure < triggerpressure){
// run the servo
unsigned long currentMillis = millis();
if (currentMillis - previousMillis > interval){
previousMillis = currentMillis; // store millis
myservo.write(angle--);
}
if (angle==maxangle){ // when the servo is at it's max angle, stop measurment
done = 1;
}
}
Serial.print('[');
Serial.print((float) millis()-starttime); Serial.print(' '); // print time since test started
Serial.print(strainV); Serial.print(' '); // print strain
Serial.print(pressure); Serial.print(' '); // print pressure
Serial.print(myservo.read()); Serial.print(' '); // print serial position
Serial.print(done);
Serial.println(']'); // Terminator statement
}
}
void curl(){
switch (curlPattern) {
case 98:
// b. light curl back
servoY.write(median+30);
break;
case 99:
// c. heavy curl back
servoY.write(maxRange);
break;
case 100:
// d. light curl front
servoY.write(median-30);
break;
case 101:
// e. heavy curl front
servoY.write(minRange);
break;
default:
// a. straight down
servoY.write(median);
}
}
void loop()
{
for(pos = 0; pos < 120; pos += 1) // goes from 0 degrees to 180 degrees
{ // in steps of 1 degree
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(1); // waits 15ms for the servo to reach the position
}
for(pos = 120; pos>=1; pos-=1) // goes from 180 degrees to 0 degrees
{
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(1); // waits 15ms for the servo to reach the position
}
for(pos = 1; pos < 120; pos += 1) // goes from 0 degrees to 180 degrees
{ // in steps of 1 degree
yourservo.write(pos); // tell servo to go to position in variable 'pos'
delay(1); // waits 15ms for the servo to reach the position
}
for(pos = 120; pos>=1; pos-=1) // goes from 180 degrees to 0 degrees
{
yourservo.write(pos); // tell servo to go to position in variable 'pos'
delay(1); // waits 15ms for the servo to reach the position
}
}
void loop () {
long duration, cm;
// send the signal, starting with LOW for a clean signal
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
duration = pulseIn(inPin, HIGH);
cm = msToCm(duration);
Serial.print(cm);
Serial.print("cm");
Serial.println();
if (cm <= 18) {
servo1.write(cm * 10);
} else if (cm <= 36) {
servo1.write((cm / 2) * 10);
} else if (cm <= 54) {
servo1.write((cm / 3) * 10);
} else {
servo1.write(cm);
}
delay(300);
}
// Spin motor at given power [-100, 100]
void rotateMotor(int pwr)
{
int val = constrain(pwr, -100, 100);
val = map(pwr, -100, 100, SERVO_MIN, SERVO_MAX);
#ifdef SERVO_WRITEUS
if(pwr == 0)
{
myservo.writeMicroseconds(SERVO_MIDPOINT);
}
else
{
myservo.writeMicroseconds(val);
}
#else
if(pwr == 0)
{
myservo.write(SERVO_MIDPOINT);
}
else
{
myservo.write(val);
}
#endif
}
// Look round the area bounded by minX,minY - maxX,maxY
// x,y is the current position, it will be incremented by incrX,incrY
bool pan()
{
if (x < 0) x = minX-incrX;
if (y < 0) y = minY-incrY;
x += incrX;
if (x == maxX && y == maxY) return 1;
if (x >= maxX || x <= minX) incrX = -incrX;
if (x == minX || x == maxX)
{
y += incrY;
if (y <= minY || y >= maxY) incrY = -incrY;
}
if (x < minX) x = minX;
if (x > maxX) x = maxX;
if (y < minY) y = minY;
if (y > maxY) y = maxY;
xServo.write(x); // tell X servo to go to position in variable 'x'
yServo.write(y); // tell Y servo to go to position in variable 'y'
delay(speed); // waits for the servo to reach the position
Serial.print(x);
Serial.write(",");
Serial.print(y);
Serial.write(" -> ");
Serial.print(maxX);
Serial.write(",");
Serial.print(maxY);
Serial.println("");
return x == maxX && y == maxY ? 1 : 0;
}
void panCheck()
{
if (sensor1 > sensor2 )
{
if (panPos >= 170)
{
panPos = 15;
panServo.write(panPos);
}
else
{
panPos += bigstep;
panServo.write(panPos);
}
}
else if (sensor1 < sensor2)
{
if (panPos <= 10)
{
panPos = 165;
panServo.write(panPos);
}
else
{
panPos -= bigstep;
panServo.write(panPos);
}
}
}
void tiltCheck()
{
if (sensor3 > sensor4 )
{
if (tiltPos >= 170)
{
tiltPos = 15;
tiltServo.write(tiltPos);
}
else
{
tiltPos += bigstep;
tiltServo.write(tiltPos);
}
}
else if (sensor3 < sensor4)
{
if (tiltPos <=10)
{
tiltPos = 165;
tiltServo.write(tiltPos);
}
else
{
tiltPos -= bigstep;
tiltServo.write(tiltPos);
}
}
}
// Move the arm down, keeping wrist level
void down_seek(int delay_time, int use_limit)
{
int pos;
for(pos = arm_position; pos < ARM_DN_POSITION; pos += 1) // We start up and increment down
{
wrist_position = map(pos, 26, 150, 100, 105); // Remap the wrist end points to something sane for the arm
shoulder.write(pos); // Write it to the servo
elbow.write(pos); // Write it to the servo
wrist.write(wrist_position); // Write it to the servo
delay(delay_time); // Slow down the loop according to specified time
arm_position = pos; // Track the arm position
if ((digitalRead(ACTUATOR_LIMIT) == LOW) && use_limit) // If using the limit then look for the switch activation
{
delay(50); // Wait a bit for debounce
if (digitalRead(ACTUATOR_LIMIT) == LOW) // If it's still low then assume it's a good trigger and we're at the bottom
{
shoulder.write(pos + 5); // Move down a bit more to make sure it's seated on the disk
elbow.write(pos + 5);
arm_position = pos + 5;
break; // Bail out of the loop since we're at the end
}
}
}
Serial.println("OK"); // Report all went well
}
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 pause(int Us)
{
setR = 90; //servo1 stop
setL = 90; //servo2 stop
servoR.write(setR); //feed servos speed setting
servoL.write(setL);
delay(Us);
}
// callback for servo position, multiple subscribesr didnt work yet...
void servoCallback(const robby_track2::pwmDirectional2& cmd_msg)
{
servoHorPos = cmd_msg.pwmDirection1;
servoVertPos = cmd_msg.pwmDirection2;
servoHor.write(servoHorPos);
servoVert.write(servoVertPos);
}
void Coil::vend()
{
servo.attach(_motorPin);
servo.write(0);
delay(1350);
servo.write(90);
setStock(getStock() - 1);
}
void cameraMove(int a,int b){
if(hordeg+a>=0&&hordeg+a<=180)hordeg+=a;
if(verdeg+b>=45&&verdeg+b<=180)verdeg+=b;
hor.write(hordeg);
ver.write(verdeg);
delay(20);
Serial.flush();
}
// Move the servo to the down position
// Do a little "jitter" so that even if moving from down->down it is visible
// that "something was done"
void moveServoDown( uint16_t offset )
{
tea_servo.write( position_down );
delay( offset * 10 );
tea_servo.write( position_down - offset );
delay( offset * 10 );
tea_servo.write( position_down );
}
void JgBigFootRobot::animateEyes() {
servoEyes.write(_degEyes - 20);
delay(300);
servoEyes.write(_degEyes);
delay(300);
servoEyes.write(_degEyes + 20);
delay(300);
servoEyes.write(_degEyes);
}
void tiltTwitch()
{
sensor3 = analogRead(lightPin);
tiltServo.write(tiltPos + littlestep);
delay(200);
sensor4 = analogRead(lightPin);
tiltServo.write(tiltPos);
delay(200);
}
void panTwitch()
{
sensor1 = analogRead(lightPin);
panServo.write(panPos + littlestep);
delay(200);
sensor2 = analogRead(lightPin);
panServo.write(panPos);
delay(200);
}
void xmove(char x) {
if(x == 'u'){
zServo.write(zServoAngle1);
delay(1000);
}else if(x == 'd'){
zServo.write(zServoAngle0);
delay(1000);
}
}
// This is called repeatedly in an event loop. The loop checks
// for the button press event. When it is pressed, the next light
// in the sequence is turned on (and the others are turned off).
//
void loop()
{
// Used to turn the servo back and forth
//
int adjust = 1;
int angle = 0;
// Default location
//
angle = 0;
tilt.write(90);
pan.write(angle);
::delay(100);
// Sample the requested area of the view sphere
//
while (true)
{
// Update the state
//
gyroscope.update();
// Log debugging output
//
::Serial.print(millis());
::Serial.print(",");
::Serial.print(angle);
::Serial.print(",");
::Serial.println(gyroscope.z(), DEC);
::Serial.flush();
// Adjust the pattern if we are at an end
//
if (angle >= 175)
{
adjust = -1;
}
else if (angle <= 5)
{
adjust = 1;
}
// Update the angle
//
angle += adjust;
// Set the pan
//
pan.write(angle);
// Add a small delay
//
::delay(10);
}
}
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();
}
// Turn Primary arm in to an specific angle between -45 to 45 degree:
// Status: TESTED
void primaryArmPosition(int Angle) {
if(Angle > 45||Angle < -45) return;
else{
SERVO_FRONT.write(NUM_SERVO_FRONT_ZERO_DEGREE - Angle);
SERVO_BACK.write(NUM_SERVO_BACK_ZERO_DEGREE + Angle);
}
}
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);
}
/* Starting angles for each servo
* degFootRight: increase = clockwise
* degHipRight: increase = <- decrease = ->
* degHipLeft: increase = <- decrease = ->
*/
void JgBigFootRobot::calibrate(int degFootRight, int degHipRight, int degFootLeft, int degHipLeft, int degEyes) {
servoFootRight.write(degFootRight);
_degFootRight = degFootRight;
servoHipRight.write(degHipRight);
_degHipRight = degHipRight;
servoFootLeft.write(degFootLeft);
_degFootLeft = degFootLeft;
servoHipLeft.write(degHipLeft);
_degHipLeft = degHipLeft;
servoEyes.write(degEyes);
_degEyes = degEyes;
}
void turn(int degreees){
if(degreees>180){
right.write(180);
left.write(180);
delay(degreees*8);
}
else{
right.write(0);
left.write(0);
delay(degreees*8);
}
}
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 posicionmin(int servo){
switch(servo){
case 1:
servo1.write(0);
break;
case 2:
servo2.write(0);
break;
case 3:
servo3.write(0);
break;
}
}
void CreateLingkaran(int nsisi, int sisi){
// turun
zServo.write(zServoAngle0);
delay(1000);
Serial.println("Printing Lingkaran ...");
printbPolygon(nsisi, sisi);
printbPolygon(nsisi, sisi);
printbPolygon(nsisi, sisi);
// angkat
delay(100);
Serial.println("Done.");
zServo.write(zServoAngle1);
delay(1000);
}
