void initservo() {
int pos = 0;
for(pos = 0; pos < 180; 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'
sdelay(20);
}
sdelay(100);
for(pos = 180; pos>=1; pos-=1) // goes from 180 degrees to 0 degrees
{
myservo.write(pos); // tell servo to go to position in variable 'pos'
sdelay(20);
SoftwareServo::refresh();
}
}
void fullStop() {
digitalWrite(STANDBY, LOW);
digitalWrite(STATUS_LED, LOW); // indicate status
stopMotor(MOTA);
stopMotor(MOTB);
myservo.write(0);
SoftwareServo::refresh();
status = CMD_STOP;
}
void IO::write(int i, int val) { // the problem might be related to the two writers that are calling this class.
SoftwareServo* servo;
// servo = &servos[i];
// servo->write(val);
switch(i) {
case 0:
servo = &s1;
break;
case 1:
servo = &s2;
break;
}
servo->write(force[val]);
servo->refresh();
// SoftwareServo::refresh();
// servo->refresh();
// delay(100);
}
void setMotor(char mot, char dir, int spd) {
analogWrite (MOTs[mot + 0], spd);
digitalWrite(MOTs[mot + 1], dir & 0x01);
digitalWrite(MOTs[mot + 2], dir & 0x02);
myservo.write(spd/2);
}
void open_claw()
{
claw_servo.write(claw_open_pos);
claw_closed = 0;
}
void close_claw()
{
claw_servo.write(claw_close_pos);
claw_closed = 1;
}
