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 detach() {
Serial2.println("detaching");
servo1.detach();
servo2.detach();
ASSERT(!servo1.attached());
ASSERT(!servo2.attached());
}
//function to call periodically to refresh the values red by the radar
void radarRefresh(){
unsigned long time=millis();
static unsigned long time_prev_rad=0;
static int pos_rad = RAD_POS_MIN, etat_rad = 0,sens_rad=1;
// gestion radar
if(etat_rad) {
if((time-time_prev_rad)>=RAD_TIMER_1) {
if ((time-time_prev_rad)>=RAD_TIMER_1+RAD_TIMER_1/2) time_prev_rad=time-RAD_TIMER_1; //to avoid problems due to long loop
time_prev_rad = time_prev_rad + RAD_TIMER_1;
// get the ranges
for(int nb = 0; nb<RAD_NB_SENSORS; nb++) {
C_rad[ (pos_rad-RAD_POS_MIN)/RAD_POS_INC + (RAD_NB_SENSORS-1-nb)*RAD_NB_POS] = getRangeResult(nb);
}
if(servoRad.attached()){
//move the servo
if(sens_rad && pos_rad < RAD_POS_MAX)
{
pos_rad = pos_rad + RAD_POS_INC;
if(pos_rad == RAD_POS_MAX) sens_rad = 0;
}
else if(!sens_rad && pos_rad > RAD_POS_MIN)
{
pos_rad = pos_rad - RAD_POS_INC;
if(pos_rad == RAD_POS_MIN) sens_rad = 1;
}
servoRad.write(pos_rad);
}
etat_rad = 0;
#ifdef DEBUG_RADAR
int i;
for (i=0;i<RAD_NB_PTS;i++) {
Serial.print(C_rad[i]);
Serial.print("|");
Serial.print(C_rad_limit[i]);
Serial.print(",\t");
}
Serial.println(" ");
#endif
}
}
else {
if((time-time_prev_rad)>=RAD_TIMER_2) {
if ((time-time_prev_rad)>=RAD_TIMER_2+(RAD_TIMER_2)/2) time_prev_rad=time-RAD_TIMER_2; //to avoid problems due to long loop
time_prev_rad = time_prev_rad + RAD_TIMER_2;
// start the ranges
for(int nb = 0; nb<RAD_NB_SENSORS; nb++) {
startRange(nb);
}
etat_rad = 1;
}
}
}
void ServoFirmata::detach(byte pin)
{
Servo *servo = servos[PIN_TO_SERVO(pin)];
if (servo) {
if (servo->attached())
servo->detach();
free(servo);
servos[PIN_TO_SERVO(pin)]=NULL;
}
}
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));
}
/** \brief Writes a value to one of the servo headers to control a servo or electronic speed dontroller
\param[in] servo A servo object to write to
\param[in] value Value from -500 to 500 representing the full range of the servo or ESC
\param[out] boolean Returns 1 if servo successfulyy written, or 0 if servo is not initialized
**/
int RCadapter::writeServo(Servo &servo, int value)
{
if (!servo.attached()) { return 0;}
servo.writeMicroseconds(value);
return 1;
}
void setup() {
//debug
Serial.begin(9600);
//pin
pinMode(resetPin, OUTPUT);
pinMode(sdcPin, OUTPUT);
//pinMode(ethPin, OUTPUT);
pinMode(greLed, OUTPUT);
pinMode(redLed, OUTPUT);
// initialize SD card
//digitalWrite(ethPin, LOW);
//digitalWrite(sdcPin, HIGH);
Serial.println("Initializing SD card...");
if (!SD.begin(sdcPin)) {
Serial.println("ERROR - SD card initialization failed!");
return;
}
// check for file
for (int i=0; i<HTTP_FILE; i++) {
if (!SD.exists(GET[i])) {
Serial.print("ERROR - Can't find ");
Serial.println(GET[i]);
return; // can't find index file
}
}
if (!SD.exists(LOG)) {
Serial.print("ERROR - Can't find ");
Serial.println(LOG);
return; // can't find index file
}
else {
dataFile = SD.open(LOG, FILE_READ);
if(dataFile) {
char temp[3],c,d;
while(dataFile.available()) { // X=123-Y=45-
d = (char) dataFile.read(); // X or Y
dataFile.read(); // =
c = (char) dataFile.read();
for(int i=0;c!='-';i++) {
temp[i] = c;
c = (char) dataFile.read();
}
String Coord(temp);
if(d=='X') {
Update('X',Coord.toInt());
}
else if(d=='Y') {
Update('Y',Coord.toInt());
}
memset(temp, 0, sizeof temp);
}
dataFile.close();
}
}
Serial.println("SUCCESS - SD card initialized.");
// initialize Ethernet device
//digitalWrite(sdcPin, LOW);
//digitalWrite(ethPin, HIGH);
Ethernet.begin(mac, ip, gateway, subnet);
server.begin();
Serial.println("SUCCESS - Ethernet initialized.");
// servo Pin
servoX.attach(xPin);
servoY.attach(yPin);
if(servoX.attached() && servoY.attached()) {
Serial.println("SUCCESS - Servo initialized.");
}
else {
Serial.println("ERROR - Servo initialization failed!");
return;
}
digitalWrite(redLed,LOW);
digitalWrite(greLed,LOW);
Serial.println("START");
}
void loop()
{
myservo.attach(pin); // attaches the servo on pin to the servo object
myservo2.attach(pin2);
myservo2.write(0);
Log("ServoIndex: %d\n", myservo2.read());
Log("ServoIndex in Microseconds: %d\n", myservo2.readMicroseconds());
delay(delayAmount);
myservo2.write(180);
Log("ServoIndex: %d\n", myservo2.read());
Log("ServoIndex in Microseconds: %d\n", myservo2.readMicroseconds());
/* Tested to work on 9/26 at 4:41pm */
myservo.write(-90);
Log("ServoIndex: %d\n", myservo.read());
Log("ServoIndex in Microseconds: %d\n", myservo.readMicroseconds());
delay(delayAmount);
myservo.write(0);
Log("ServoIndex: %d\n", myservo.read());
Log("ServoIndex in Microseconds: %d\n", myservo.readMicroseconds());
delay(delayAmount);
myservo.write(180);
Log("ServoIndex: %d\n", myservo.read());
Log("ServoIndex in Microseconds: %d\n", myservo.readMicroseconds());
delay(delayAmount);
myservo.write(200);
Log("ServoIndex: %d\n", myservo.read());
Log("ServoIndex in Microseconds: %d\n", myservo.readMicroseconds());
delay(delayAmount);
/*Tested to work on 9/26 at 4:44pm */
myservo.writeMicroseconds(544);
Log("ServoIndex: %d\n", myservo.read());
Log("ServoIndex in Microseconds: %d\n", myservo.readMicroseconds());
delay(delayAmount);
myservo.writeMicroseconds(4000);
Log("ServoIndex: %d\n", myservo.read());
Log("ServoIndex in Microseconds: %d\n", myservo.readMicroseconds());
delay(delayAmount);
Log("ServoIndex: %d\n", myservo.read());
Log("ServoIndex in Microseconds: %d\n", myservo.readMicroseconds());
if (myservo.attached())
{
Log("Servo is attached\n");
Log("Servo is detaching\n");
myservo.detach();
if (!myservo.attached())
{
Log("Servo is detached\n");
}
}
else
{
Log("Servo is not attached\n");
}
//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'
// delay(15); // waits 15ms for the servo to reach the position
//}
//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'
// delay(15); // waits 15ms for the servo to reach the position
//}
}
