#include <SPI.h>
#include <WiShield.h>
#include <Ethernet.h>
#include <Servo.h>
#include "Setter.h"
#include "Inverter.h"
#include "Limiter.h"
#include "Linear.h"
#include "ServoSetter.h"
#include "AnalogSetter.h"
#include "HBridgeSetter.h"
#include "DigitalSetter.h"
#include "AccelStepper.h"
#include "AccelStepperSpeedSetter.h"
#include "AccelStepperPositionSetter.h"
#include "DifferentialSetter.h"
#include "MotorControllerSetter.h"
#include "SetterDebug.h"
#define ANALOG_TEST_PINA 0
#define ANALOG_TEST_PINB 1
#define DIGITAL_TEST_PIN 2
Servo servo;
AccelStepper stepper;
void setup()
{
Serial.begin(9600);
pinMode(DIGITAL_TEST_PIN, OUTPUT);
stepper.setMaxSpeed(1000);
}
unsigned int errors = 0;
void error(const char *m)
{
Serial.print("Error: ");
Serial.println(m);
errors++;
}
void check(boolean t, const char *m)
{
if (!t)
error(m);
}
void loop()
{
s1.input(100);
s1.input(200);
check(!d1.
failed(),
"basic 3");
s1.failsafe();
check(d1.
failed(),
"basic 4");
l1.setTarget(&d1);
l1.input(300);
l1.input(120);
l1.input(115);
l1.input(100);
lin1.setTarget(&d1);
lin1.input(0);
lin1.input(10);
lin1.input(100);
lin1.input(-100);
servo.attach(2);
ss1.input(0);
check(servo.read() == 0, "ServoSetter 1");
ss1.input(128);
check(servo.read() == 90, "ServoSetter 2");
ss1.input(255);
check(servo.read() == 180, "ServoSetter 3");
as1.input(0);
as1.input(128);
as1.input(255);
hs1.input(127);
hs1.input(200);
hs1.input(255);
hs1.input(100);
hs1.input(0);
ds1.input(0);
check(digitalRead(DIGITAL_TEST_PIN) == LOW, "DigitalSetter 1");
ds1.input(100);
check(digitalRead(DIGITAL_TEST_PIN) == HIGH, "DigitalSetter 2");
asss1.input(0);
check(stepper.speed() == 0.0, "AccelStepperSpeedSetter 1");
asss1.input(100);
check(stepper.speed() == 100.0, "AccelStepperSpeedSetter 2");
asps1.input(0);
check(stepper.targetPosition() == 0, "AccelStepperPositionSetter 1");
asps1.input(100);
check(stepper.targetPosition() == 100, "AccelStepperPositionSetter 2");
diff1.input(127);
di1.input(127);
check(d1.
lastValue() == 127,
"DifferentialSetter 1");
check(d2.
lastValue() == 127,
"DifferentialSetter 2");
di1.input(157);
check(d1.
lastValue() == 157,
"DifferentialSetter 3");
check(d2.
lastValue() == 97,
"DifferentialSetter 4");
di1.input(107);
check(d1.
lastValue() == 107,
"DifferentialSetter 5");
check(d2.
lastValue() == 147,
"DifferentialSetter 6");
diff1.input(255);
di1.input(127);
check(d1.
lastValue() == 255,
"DifferentialSetter 7");
check(d2.
lastValue() == 255,
"DifferentialSetter 8");
di1.input(147);
check(d1.
lastValue() == 255,
"DifferentialSetter 9");
check(d2.
lastValue() == 215,
"DifferentialSetter 10");
di1.input(107);
check(d1.
lastValue() == 215,
"DifferentialSetter 11");
check(d2.
lastValue() == 255,
"DifferentialSetter 12");
diff1.input(0);
di1.input(127);
check(d1.
lastValue() == 0,
"DifferentialSetter 13");
check(d2.
lastValue() == 0,
"DifferentialSetter 14");
di1.input(147);
check(d1.
lastValue() == 40,
"DifferentialSetter 15");
check(d2.
lastValue() == 0,
"DifferentialSetter 16");
di1.input(107);
check(d1.
lastValue() == 0,
"DifferentialSetter 17");
check(d2.
lastValue() == 40,
"DifferentialSetter 18");
l1.setTarget(&i2);
check(stepper.targetPosition() == -110, "Chaining 1");
check(stepper.targetPosition() == -120, "Chaining 2");
mc1.input(0);
check(d1.
lastValue() == 0,
"MotorControllerSetter 1");
check(d2.
lastValue() == 254,
"MotorControllerSetter 2");
mc1.input(100);
check(d1.
lastValue() == 0,
"MotorControllerSetter 1");
check(d2.
lastValue() == 54,
"MotorControllerSetter 2");
mc1.input(127);
check(d1.
lastValue() == 255,
"MotorControllerSetter 3");
check(d2.
lastValue() == 0,
"MotorControllerSetter 4");
mc1.input(128);
check(d1.
lastValue() == 255,
"MotorControllerSetter 3");
check(d2.
lastValue() == 2,
"MotorControllerSetter 4");
mc1.input(200);
check(d1.
lastValue() == 255,
"MotorControllerSetter 5");
check(d2.
lastValue() == 146,
"MotorControllerSetter 6");
mc1.input(255);
check(d1.
lastValue() == 255,
"MotorControllerSetter 5");
check(d2.
lastValue() == 256,
"MotorControllerSetter 6");
Serial.print("Done. Errors: ");
Serial.println(errors, DEC);
pinMode(13, OUTPUT);
if (errors)
{
digitalWrite(13, HIGH);
delay(500);
digitalWrite(13, LOW);
delay(500);
digitalWrite(13, HIGH);
delay(500);
digitalWrite(13, LOW);
}
else
{
digitalWrite(13, HIGH);
delay(500);
digitalWrite(13, LOW);
}
while (1)
;
}