122 lines
2.8 KiB
C++
122 lines
2.8 KiB
C++
/*
|
|
ServoTest.cpp
|
|
(C)2013 kou029w - MIT License
|
|
*/
|
|
|
|
#include <Arduino.h>
|
|
#include <Servo.h>
|
|
#include "ServoTest.h"
|
|
#include "SerialUtil.h"
|
|
|
|
//#ifdef Servo_h
|
|
template<>
|
|
class ServoTestClass<Servo,HardwareSerial>;
|
|
#ifdef __AVR_ATmega32U4__
|
|
template<>
|
|
class ServoTestClass<Servo,Serial_>;
|
|
#endif
|
|
//#endif
|
|
|
|
template<typename SERVO, typename S>
|
|
void ServoTestClass<SERVO,S>::_setAndPrint(SERVO& servo, const char* servoName, S& serial){
|
|
static unsigned long lastTime;
|
|
lastTime = _time;
|
|
_time = millis();
|
|
serial.print("delay(");
|
|
serial.print(_time - lastTime);
|
|
serial.println(");");
|
|
|
|
serial.print(servoName);
|
|
switch(_mode){
|
|
case ATTACH_MODE:
|
|
serial.print(".attach(");
|
|
serial.print(_val);
|
|
servo.attach(_val);
|
|
break;
|
|
case DETACH_MODE:
|
|
serial.println(".detach(");
|
|
servo.detach();
|
|
break;
|
|
case WRITE_MODE:
|
|
serial.print(".write(");
|
|
serial.print(_val);
|
|
servo.write(_val);
|
|
break;
|
|
case WRITE_MICROSEC_MODE:
|
|
serial.print(".writeMicroseconds(");
|
|
serial.print(_val);
|
|
servo.writeMicroseconds(_val);
|
|
break;
|
|
}
|
|
serial.println(");");
|
|
}
|
|
|
|
template<typename SERVO, typename S>
|
|
void ServoTestClass<SERVO,S>::operator()(S& serial){
|
|
// using namespace servotest;
|
|
char _helpdoc[] = ""
|
|
"/*\n"
|
|
"## help ##\n"
|
|
"default\n"
|
|
" Format : \"angle + 'a/b'\"\n"
|
|
" Example: \"90a\" => servoA.write(90)\n"
|
|
"\n"
|
|
"[t] attach\n"
|
|
" Format : \"'t' + pin + 'a/b'\"\n"
|
|
" Example: \"t8b\" => servoB.attach(8)\n"
|
|
"\n"
|
|
"[d] detach\n"
|
|
" Format : \"'d' + 'a/b'\"\n"
|
|
" Example: \"db\" => servoB.detach()\n"
|
|
"\n"
|
|
"[u] write-microseconds\n"
|
|
" Format : \"'u' + us + 'a/b'\"\n"
|
|
" Example: \"u1500a\" => servoA.writeMicroseconds(1500)\n"
|
|
"*/\n";
|
|
_time = millis();
|
|
SERVO _servoA;
|
|
SERVO _servoB;
|
|
_servoA.attach(9);
|
|
_servoB.attach(10);
|
|
serial.println("servoA.attach(9);");
|
|
serial.println("servoB.attach(10);");
|
|
|
|
/* Main loop */
|
|
for(;;){
|
|
_mode = NONE;
|
|
_val = 0;
|
|
serial.println("/*-- [q] quit [h] help --*/");
|
|
while(_mode == NONE){
|
|
char c = getStr(1, serial)[0];
|
|
switch(c){
|
|
case 'h':
|
|
serial.print(_helpdoc);
|
|
break;
|
|
case 'q':
|
|
_servoA.detach();
|
|
_servoB.detach();
|
|
return;
|
|
case 't':
|
|
_mode = ATTACH_MODE;
|
|
break;
|
|
case 'd':
|
|
_mode = DETACH_MODE;
|
|
break;
|
|
case 'u':
|
|
_mode = WRITE_MICROSEC_MODE;
|
|
break;
|
|
case '0'...'9':
|
|
_val = _val * 10 + c - '0';
|
|
break;
|
|
case 'a':
|
|
if(_mode == NONE) _mode = WRITE_MODE;
|
|
_setAndPrint(_servoA, "servoA", serial);
|
|
break;
|
|
case 'b':
|
|
if(_mode == NONE) _mode = WRITE_MODE;
|
|
_setAndPrint(_servoB, "servoB", serial);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|