void detach() {
Serial2.println("detaching");
servo1.detach();
servo2.detach();
ASSERT(!servo1.attached());
ASSERT(!servo2.attached());
}
void mode_test(){
SERIAL_OUT.println();
SERIAL_OUT.println();
SERIAL_OUT.println("toggle mode (i.e. TX CH3) and this function will print its current state");
SERIAL_OUT.println("perform hard reset to exit function");
esc.detach();
pinMode(THROTTLE, OUTPUT);
digitalWrite(THROTTLE, LOW);
bool attached = true;
long time_temp = 0;
while(1){
get_mode();
if(mode == MANUAL){
if(attached == true){
esc.detach();
// pinMode(THROTTLE, OUTPUT);
// digitalWrite(THROTTLE, LOW);
attached = false;
}
}
else if(mode == AUTOMATIC){
if(attached == false){
delay(250);
esc.attach(THROTTLE);
delay(250);
esc.writeMicroseconds(S_STOP);
attached = true;
}
}
else if(mode == AUX){
if(attached == true){
esc.detach();
// pinMode(THROTTLE, OUTPUT);
// digitalWrite(THROTTLE, LOW);
attached = false;
}
}
else if(mode == WP_MODE){
if(attached == true){
esc.detach();
// pinMode(THROTTLE, OUTPUT);
// digitalWrite(THROTTLE, LOW);
attached = false;
}
}
else SERIAL_OUT.println("nothing valid detected");
if((millis() - time_temp) > 250){
SERIAL_OUT.println(mode);
time_temp = millis();
}
}
return ;
}
void loop() {
int digit = counter;
int dig1 = digit % 10;
digit /= 10;
int dig2 = digit % 10;
digit /= 10;
int dig3 = digit % 10;
digit /= 10;
int dig4 = digit % 10;
digit /= 10;
int dig5 = digit % 10;
lc.clearDisplay(0);
//lc.setDigit(0,7,0,false);
//lc.setDigit(0,6,0,false);
//lc.setDigit(0,5,0,false);
lc.setDigit(0,4,dig5,false);
lc.setDigit(0,3,dig4,false);
lc.setDigit(0,2,dig3,false);
lc.setDigit(0,1,dig2,false);
lc.setDigit(0,0,dig1,false);
// up
if(state == 0) {
if(counter > 0) {
counter --;
} else {
pos=100;
myservo.attach(servopin);
myservo.write(160);
delay(200);
myservo.write(140);
delay(200);
myservo.write(120);
delay(200);
myservo.write(pos);
delay(200);
myservo.detach();
state = 1;
counter = low;
}
// down
} else if(state ==1) {
if(counter > 0) {
counter --;
} else {
pos=180;
myservo.attach(servopin);
myservo.write(pos);
delay(500);
myservo.detach();
state = 0;
counter = high;
}
}
delay(delaytime);
}
void servo_test(){
steering.attach(STEERING);
steering.writeMicroseconds(STEER_ADJUST);
esc.attach(THROTTLE);
esc.write(90);
SERIAL_OUT.println();
SERIAL_OUT.println();
SERIAL_OUT.println("set CH3 to AUTOMATIC");
while(mode != AUTOMATIC) get_mode();
while(mode == AUTOMATIC){
SERIAL_OUT.println("normal angle output");
for(int pos = 30; pos < 150; pos += 1){ //goes from 0 degrees to 180 degrees in steps of 1 degree
steering.write(pos); //tell servo to go to position in variable 'pos'
SERIAL_OUT.println(pos);
delay(15); //waits 15ms for the servo to reach the position
}
for(int pos = 150; pos >= 30; pos -= 1){ //goes from 180 degrees to 0 degrees
steering.write(pos); //tell servo to go to position in variable 'pos'
SERIAL_OUT.println(pos);
delay(15); //waits 15ms for the servo to reach the position
}
SERIAL_OUT.println("microsecond angle output");
for(int pos = 1250; pos < 1750; pos += 1){ //goes from 0 degrees to 180 degrees in steps of 1 degree
steering.writeMicroseconds(pos); //tell servo to go to position in variable 'pos'
SERIAL_OUT.println(pos);
delay(5); //waits 15ms for the servo to reach the position
}
for(int pos = 1750; pos >= 1250; pos -= 1){ //goes from 180 degrees to 0 degrees
steering.writeMicroseconds(pos); //tell servo to go to position in variable 'pos'
SERIAL_OUT.println(pos);
delay(5); //waits 15ms for the servo to reach the position
}
get_mode();
}
esc.detach();
steering.detach();
return;
}
void steering_calibration(){
esc.detach();
SERIAL_OUT.println();
angle_target = 0.0;
steering.attach(STEERING);
steering.writeMicroseconds(STEER_ADJUST);
delay(500);
setup_mpu6050();
calculate_null();
SERIAL_OUT.println("set controller to automatic");
get_mode();
while(mode != AUTOMATIC) get_mode();
accum = 0; //this is ONLY used to reset the 0 the gyro angle for real (setting angle to 0 does nothing!!! (never forget last year's debacle))
delay(250);
esc.attach(THROTTLE);
delay(1000);
esc.writeMicroseconds(S_STOP);
delay(1000);
esc.writeMicroseconds(S_LOW);
while(mode == AUTOMATIC){
read_FIFO();
update_steering();
steering.writeMicroseconds(steer_us);
if((millis() - time) > 500){
SERIAL_OUT.print("angle: ");
SERIAL_OUT.print((angle*180.0/3.1415),5);
SERIAL_OUT.print("\tsteering ms: ");
SERIAL_OUT.println(steer_us);
time = millis();
}
get_mode();
}
steering.detach();
esc.detach();
return ;
}
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 lockEventTick()
{
if (lockTransitionTimeCounter)
{
lockTransitionTimeCounter--;
if (lockTransitionTimeCounter == 0)
servo.detach();
}
if (doorRevertCounter)
doorRevertCounter--;
}
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));
}
void ManageServo::Detach(Servo servo)
{
servo.detach();
}
void loop() {
float vol_value = analogRead(VOLPIN);
char str[5];
int val;
if(isDebug == 0 && isServoDone == 0){
//for button
buttonState = digitalRead(buttonPin);
if (buttonState == HIGH) {
//ボタンが押されたらサーボ起動、移動後にFlahsAirの初期化 (TODO 先に初期化するとサーボが動かないバグのため暫定)
Serial.println("Btn High!!!!yeah");
//時計周りに 0 度の方向へ、そして戻した後に、FAを起動
moveServoShaft(0, 1);
moveServoShaft(90, 1);
delay(1000);
isDebug = 1;
// Initialize SD card.
Serial.print(F("\nInitializing SD card..."));
if (card.init(SPI_HALF_SPEED, chipSelectPin)) {
Serial.print(F("OK"));
} else {
Serial.print(F("NG"));
abort();
}
memset(buffer, 0, 0x200);
}else {
//nothing
}
int d = Serial.read();
if( d == 'a' ){
sv.attach(SERVO_PIN);
}
else if( d == 'd' ){
sv.detach();
}
else if( d == 'r' ){
Serial.println(sv.read());
Serial.println(sv.readMicroseconds());
}
else if( '0' <= d && d <= '9' ){
//時計周りに 0 度の方向へそして戻してFA移動
moveServoShaft(0, 10);
moveServoShaft(90, 10);
delay(1000);
//サーボ終了
isServoDone = 1;
// Initialize SD card.
Serial.print(F("\nInitializing SD card..."));
if (card.init(SPI_HALF_SPEED, chipSelectPin)) {
Serial.print(F("OK"));
} else {
Serial.print(F("NG"));
abort();
}
memset(buffer, 0, 0x200);
}
//}
}else{
if (card.readExtMemory(1, 1, 0x1000, 0x200, buffer)) {
str[0] = buffer[0];
str[1] = buffer[1];
str[2] = buffer[2];
str[3] = buffer[3];
str[4] = 0;
val = atoi(str);
//0バイトの場合初期化
if(currentByte != 0 && val == 0){
strip.clear();
strip.show();
Serial.println("back to start");
currentByte = 0;
}
if(val == 0 || val == currentByte){
//do nothing...
//Serial.println("get 1...");
}else{
if (vol_value < 800){
//Serial.println("byte changed...");
currentByte = val;
showLed3AndHelloServo(val);
Serial.println(val);
}else{
strip.clear();
strip.show();
}
}
}
}
delay(450);
}
void showLed3AndHelloServo(int val){
Serial.println("led array was....");
Serial.println(val);
Serial.println("ooooooooooooooooooo");
//strip.clear();
switch(val){
case 123://how
colorWipe(strip.Color(255, 255, 255), 100); // white
colorWipe(strip.Color(0, 0, 0), 100); // black
colorWipe(strip.Color(255, 255, 255), 100); // white
colorWipe(strip.Color(0, 0, 0), 100); // black
colorWipe(strip.Color(255, 255, 255), 100); // white
colorWipe(strip.Color(0, 0, 0), 100); // black
//TODO Arduinoをリセット
wdt_enable(WDTO_15MS); // turn on the WatchDog and don't stroke it.
for(;;) {
// do nothing and wait for the eventual...
}
break;
case 1://サーボ準備
sv.attach(SERVO_PIN);
Serial.println("servo attached...");//サーボの位置確認
break;
case 2://サーボ回転
Serial.println(sv.read());//サーボの位置確認
moveServoShaft(0, 10);
//delay(100);//TODO 不要
delay(1000);
break;
case 3://サーボ戻す
Serial.println(sv.read());//サーボの位置確認
moveServoShaft(90, 10);
//delay(100);//TODO 不要
delay(1000);
break;
case 4://サーボ終了
sv.detach();
break;
case 0://クリア
strip.clear();
break;
case 555://白
case 5://白
colorWipe(strip.Color(255, 255, 255), 80); // white
delay(3000);
colorWipe(strip.Color(0, 0, 0), 100); // black
break;
case 666://紫
case 6://紫
colorWipe(strip.Color(105, 14, 102), 80); // purple
delay(3000);
colorWipe(strip.Color(0, 0, 0), 100); // black
break;
case 777://ピンク
case 7://ピンク
colorWipe(strip.Color(252, 18, 245), 80); // pink
delay(3000);
colorWipe(strip.Color(0, 0, 0), 100); // black
break;
case 888://レインボー？
case 8://レインボー？
rainbow(20);
//rainbowCycle(80);
break;
case 999://黒 = 消す
case 9://黒 = 消す
colorWipe(strip.Color(0, 0, 0), 10); // black
strip.clear();
break;
case 100://予備
break;
default:
Serial.println("this is for servo");
colorWipe(strip.Color(0, 0, 0), 10); // black
strip.clear();
break;
}
}
mrb_value mrb_servo_detach(mrb_state *mrb, mrb_value self){
Servo *servo = (Servo *)mrb_get_datatype(mrb, self, &mrb_servo_type);
servo->detach();
return mrb_nil_value();
}
void loop() {
// myservo.write(pos); // tell servo to go to position in variable 'pos'
// Serial.print("pos");
// Serial.println(pos, DEC);
// if there's any serial available, read it:
str_recv = "";
while (Serial.available() > 0) {
str_recv = str_recv + Serial.readString();
//Serial.print(str_recv);
//delay(1000);
if (str_recv =="Open\n") {
// pos = 0; // send servo to 0
// Serial.println("equals");
myservo.attach(11); // attaches the servo on pin 11 to the servo object
myservo.write(0);
delay(2000); // 2 seconds to achive the desired position
myservo.detach(); // attaches the servo on pin 11 to the servo object
}
if (str_recv =="Close\n") {
myservo.attach(11); // attaches the servo on pin 11 to the servo object
myservo.write(180);
delay(2000); // 2 seconds to achive the desired position
myservo.detach(); // attaches the servo on pin 11 to the servo object
}
String stringOne = "Content-Type: text/html";
// you can also look for a substring in the middle of a string:
////Serial.println("str_recv.substring(0,4)");
//time repeat and duration of the waterings
if (str_recv.substring(0,7) =="Repeat:") {
// str_recv.replace(1, 7);
//str_aux = str_recv[7]);
int val_sec = str_recv.substring(7).toInt();
Serial.println(val_sec, DEC);//7
}
//Serial.println(str_recv);
// look for the newline. That's the end of your
// sentence:
if (Serial.read() == '\n') {
//Serial.println(str_recv);
}
}
// read the analog in value:
sensorValue = analogRead(analogInPin);
moistureVal = analogRead(moisteurPin);
// print the results to the serial monitor:
Serial.print("light = " );
Serial.print(sensorValue);
Serial.print("\t moisture = ");
Serial.println(moistureVal);
// Read values from the sensor
temp_c = sht1x.readTemperatureC();
temp_f = sht1x.readTemperatureF();
humidity = sht1x.readHumidity();
// Print the values to the serial port
Serial.print("Temperature: ");
Serial.print(temp_c, DEC);
Serial.print("C / ");
Serial.print(temp_f, DEC);
Serial.print("F. Humidity: ");
Serial.print(humidity);
Serial.println("%");
delay(10);
}
void loop() {
// try to get client
EthernetClient client = server.available();
String getStr;
if (client) {
digitalWrite(greLed,HIGH);
boolean currentLineIsBlank = true;
boolean indice = false;
while (client.connected()) {
if (client.available()) { // client data available to read
char c = client.read(); // read 1 byte (character) from client
// filter GET request
// GET /index.html HTTP/1.1
if (c == 'G') {
c = client.read();
if (c == 'E') {
c = client.read();
if (c == 'T') {
Serial.print("GET=");
c = client.read(); // space
while (true) {
c = client.read();
if (c == ' ') {
break;
}
//Serial.println(c);
getRequest[getIndi] = c;
getIndi++;
}
Serial.print(getRequest);
Serial.println("@");
}
}
}
if (c == '\n' && currentLineIsBlank) {
// convert to string and clear char array
String getStr(getRequest);
Clear();
// if check
// first page
if (getStr == "/" || getStr == "/index.html") {
indice = true;
}
// ajax GET info
else if (getStr.startsWith("/Set?")) {
indice = true;
int getRequestStart = getStr.indexOf('?' );
int getRequestFirst = getStr.indexOf('=' );
int getRequestFinal = getStr.indexOf('/',2);
int getRequestDuall = getStr.indexOf('&' ); // 2 GET
if (getRequestDuall>0) { // /index.html?X=20&Y=30
int getRequestSecon = getStr.indexOf('=', getRequestFirst+1);
String inputFirst_1 = getStr.substring(getRequestStart+1, getRequestFirst); // X
String valueFirst_1 = getStr.substring(getRequestFirst+1, getRequestDuall); // 20
String inputFirst_2 = getStr.substring(getRequestDuall+1, getRequestSecon); // Y
String valueFirst_2 = getStr.substring(getRequestSecon+1, getRequestFinal); // 30
if (inputFirst_1 == "X") {
Update('X',valueFirst_1.toInt());
Update('Y',valueFirst_2.toInt());
}
// reverse
else if (inputFirst_1 == "Y") {
Update('Y',valueFirst_1.toInt());
Update('X',valueFirst_2.toInt());
}
}
else { // /index.html?Y=50 or X=50
String inputFirst = getStr.substring(getRequestStart+1, getRequestFirst); // Y
String valueFirst = getStr.substring(getRequestFirst+1, getRequestFinal); // 50
if (inputFirst == "X") {
Update('X',valueFirst.toInt());
}
else if (inputFirst == "Y") {
Update('Y',valueFirst.toInt());
}
}
}
//ajax SAVE info
else if (getStr.startsWith("/Save/")) {
digitalWrite(redLed,HIGH);
client.println("HTTP/1.1 200 OK");
client.println("Connection: close");
Serial.println("SAVE");
Serial.println("");
SD.remove(LOG);
dataFile = SD.open(LOG, FILE_WRITE);
if(dataFile) { // X=123-Y=45-
dataFile.print("X=");
dataFile.print(X);
dataFile.print("-Y=");
dataFile.print(Y);
dataFile.print("-");
dataFile.close();
}
digitalWrite(redLed,LOW);
}
//ajax RESET info
else if (getStr.startsWith("/Reset/")) {
digitalWrite(redLed,HIGH);
client.println("HTTP/1.1 200 OK");
client.println("Connection: close");
Serial.println("RESET");
Serial.println("");
delay(1);
servoX.detach();
servoY.detach();
digitalWrite(redLed,LOW);
digitalWrite(resetPin, LOW);
}
// ajax SET info
else if (getStr.startsWith("/Coordinate/")) {
digitalWrite(redLed,HIGH);
GetValue();
double temp = analogRead(thermRes);
int phot = analogRead(photoRes);
client.println("HTTP/1.1 200 OK");
client.println("Connection: close");
client.println();
// JSON
client.print("{\"coordinate\":{\"X\":");
client.print(X);
client.print(",\"Y\":");
client.print(Y);
client.print("},\"temp\":");
client.print(GetTemp(temp),1);
client.print(",\"light\":");
client.print(phot);
client.print(",\"network\":\"");
client.print(Ethernet.localIP());
client.print("\",\"file\":[");
for (int i=0; i<HTTP_FILE; i++) {
File dFile = SD.open(GET[i]);
if (dFile) {
if(i>0) {
client.print(",");
}
client.print("{");
client.print("\"name\":\"");
client.print(GET[i]);
client.print("\",\"size\":");
client.print(dFile.size());
client.print("}");
}
}
client.println("]}");
digitalWrite(redLed,LOW);
}
// print other file
else {
for(int i=1; i<HTTP_FILE; i++) {
if (getStr == TYP[0][i]) {
webFile = SD.open(GET[i]);
if (webFile) {
client.println("HTTP/1.1 200 OK");
client.println(TYP[1][i]);
if(TYP[2][i] == "1") {
client.println("Content-Encoding: gzip");
}
client.print("Content-Length: ");
client.println(webFile.size());
client.println("Cache-Control: max-age=302400, public");
client.println("Connection: close");
client.println();
}
break;
}
}
}
// endif check
// print index.html
if (indice) {
webFile = SD.open(GET[0]);
if (webFile) {
client.println("HTTP/1.1 200 OK");
client.println(TYP[1][0]);
client.print("Content-Length: ");
client.println(webFile.size());
client.println("Connection: close");
client.println();
}
}
// read file and write into web client
if (webFile) {
while(webFile.available()) {
client.write(webFile.read());
}
webFile.close();
}
break;
}
if (c == '\n') {
// last character on line of received text. Starting new line with next character read
currentLineIsBlank = true;
}
else if (c != '\r') {
// you've gotten a character on the current line
currentLineIsBlank = false;
}
}
}
//delay(1); // give the web browser time to receive the data
client.stop(); // close the connection
digitalWrite(greLed,LOW);
}
}
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
//}
}
