diff --git a/ship/ship.ino b/ship/ship.ino index e31ae90..1a74d01 100644 --- a/ship/ship.ino +++ b/ship/ship.ino @@ -9,8 +9,8 @@ Servo MyServo; Servo RidaServo; RF24 radio(7,8); int count =0; -int IN1 =7; -int IN2 = 8; +int IN1 =9; +int IN2 =10; int PWM=6; int TrigPin=2; int EcoPin=3; @@ -21,61 +21,47 @@ float dist; long dj,z;//dj是舵机,z是螺旋桨转速 //long propeller=0,rudder=0;//propeller为螺旋桨(0-1023,512为静止),rudder为舵(0-1023,512为静止) //long s_propeller=0,s_rudder=0;//发送的信号 -const long R_prop=5,R_rudd=6;//输出端口 -long check_array[7],content,after_decode;//校验序列、读取内容和解码内容 +long propeller,rudder,content;//校验序列、读取内容和解码内容 const byte addresses[6] = "00001"; //创建一个数组,建立接收机地址,或者说两个模块将用于通信的“管道” -long decode_and_confirm(long x){ - long sum=0,add=0,x1=x; - while(x1!=0){ - check_array[add]=x1%10; - x1/=10; - add++; - } - for(long i=0,j=4;i=1000000&&x<=2000000)return x%1000000; + else return -1; } - void loop() { - //Serial.println(sizeof(long)); - analogWrite(PWM,0); - radio.startListening(); - //Serial.println("start listening!"); - - - if(angle<=0){ + + + radio.startListening(); + // Serial.println("start listening!"); + /*if(angle<=0){ dAngle=5; } else if(angle>=180){ dAngle=-5; } - angle=angle+dAngle; + /*angle=angle+dAngle; RidaServo.write(angle); digitalWrite(TrigPin,LOW); delayMicroseconds(8); @@ -88,74 +74,76 @@ void loop() { } if(dist/170){ - radio.read(&content,sizeof(content)); + radio.read(&content,sizeof(content)); Serial.println("The content is"); Serial.println(content);//调试 - after_decode=decode_and_confirm(content); - Serial.println(after_decode); - if(after_decode!=-1&&content>=100000){ - if(content<200000){ - Serial.println("inputing propeller "); - if(after_decode>600||after_decode<500){ - if(after_decode>600){ - long zheng=after_decode; - digitalWrite(IN2,HIGH); - digitalWrite(IN1,LOW); - analogWrite(PWM,zheng); - delay(50); - z=zheng-512; - Serial.println("forward propelling"); - Serial.println(z); - } - else if(after_decode<500){ - long fan=1100-after_decode; - digitalWrite(IN1,HIGH); - digitalWrite(IN2,LOW); - analogWrite(PWM,fan); - delay(50); - z=fan-512; - Serial.println("back propelling"); - Serial.println(z); - } - } - else if(after_decode<=600||after_decode>=500){ - z=0; - Serial.println("stop propelling"); - Serial.println(z); - } - } - else{ - Serial.println("inputing ruddering"); - if(after_decode>600||after_decode<500){ - if(after_decode<500){ - long fan1=0.18*after_decode; - MyServo.write(fan1/2); - delay(150); - dj=fan1-90; - Serial.println("turn left"); - Serial.println(dj); - } - else if(after_decode>600){ - long zheng1=0.22*after_decode-42; - MyServo.write(zheng1/2); - delay(150); - dj=zheng1-90; - Serial.println("turn right"); - Serial.println(dj); - } - } - else if(after_decode<=600||after_decode>=500){ - dj=0; - Serial.println("stop turing"); - Serial.println(dj); - } + content=confirm(content); + if(content!=-1){ + propeller=content%1000; + rudder=(content-propeller)/1000; + if(propeller>550||propeller<449) + { + if(propeller>550) + { + long zheng=0.568*propeller-312.4; + digitalWrite(IN2,HIGH); + digitalWrite(IN1,LOW); + analogWrite(PWM,zheng); + z=zheng; + Serial.println("forward propelling"); + Serial.println(z); + } + else if(propeller<449) + { + long fan=255-0.568*propeller; + digitalWrite(IN1,HIGH); + digitalWrite(IN2,LOW); + analogWrite(PWM,fan); + z=fan; + Serial.println("back propelling"); + Serial.println(z); } } - } + else if(propeller<=550||propeller>=449) + { + z=0; + analogWrite(PWM,0); + Serial.println("stop propelling"); + Serial.println(z); + } + Serial.println("inputing ruddering"); + if(rudder>550||rudder<449) + { + if(rudder<449) + { + long fan1=0.1*rudder; + MyServo.write(fan1); + Serial.println("turn left"); + dj=2*fan1-90; + Serial.println(dj); + } + else if(rudder>550) + { + long zheng1=0.1*rudder-10.122; + MyServo.write(zheng1); + dj=2*zheng1-90; + Serial.println("turn right"); + Serial.println(dj); + } + } + else if(rudder<=550||rudder>=449) + { + dj=0; + MyServo.write(45); + Serial.println("stop turing"); + Serial.println(dj); + } + } + } // radio.stopListening(); //radio.write(&dist,sizeof(dist)); // radio.write(&dj,sizeof(dj));