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();
}
}
// functions
void init_claw ()
{
claw_servo.attach(28); // attaches the servo on Arduino pin 28 to the SoftwareServo object
open_claw();
claw_closed = 0;
}
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 setup() {
pinMode( SPDA , OUTPUT );
pinMode( SPDB , OUTPUT );
pinMode( STANDBY , OUTPUT );
pinMode( STATUS_LED , OUTPUT );
pinMode( DIR1A , OUTPUT );
pinMode( DIR2A , OUTPUT );
pinMode( DIR1B , OUTPUT );
pinMode( DIR2B , OUTPUT );
myservo.attach(HAXXXOR); // attaches the servo pin
initservo();
fullStop(); //init's all previous outputs
Serial.begin(USBSERIAL); // init USB Serial (console)
Serial1.begin(BTGPSRATE); // init pin0/1 Serial (bluetooth)
delay(1234);
setupBT(3210);
while(readBT() != CMD_STOP); // safety frist? ¯\(°_o)/¯ I DUNNO LOL
}
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;
}
