ship new protocol
This commit is contained in:
parent
805f8de61c
commit
42b038308d
104
ship/ship.ino
104
ship/ship.ino
|
@ -9,8 +9,8 @@ Servo MyServo;
|
||||||
Servo RidaServo;
|
Servo RidaServo;
|
||||||
RF24 radio(7,8);
|
RF24 radio(7,8);
|
||||||
int count =0;
|
int count =0;
|
||||||
int IN1 =7;
|
int IN1 =9;
|
||||||
int IN2 = 8;
|
int IN2 =10;
|
||||||
int PWM=6;
|
int PWM=6;
|
||||||
int TrigPin=2;
|
int TrigPin=2;
|
||||||
int EcoPin=3;
|
int EcoPin=3;
|
||||||
|
@ -21,27 +21,12 @@ float dist;
|
||||||
long dj,z;//dj是舵机,z是螺旋桨转速
|
long dj,z;//dj是舵机,z是螺旋桨转速
|
||||||
//long propeller=0,rudder=0;//propeller为螺旋桨(0-1023,512为静止),rudder为舵(0-1023,512为静止)
|
//long propeller=0,rudder=0;//propeller为螺旋桨(0-1023,512为静止),rudder为舵(0-1023,512为静止)
|
||||||
//long s_propeller=0,s_rudder=0;//发送的信号
|
//long s_propeller=0,s_rudder=0;//发送的信号
|
||||||
const long R_prop=5,R_rudd=6;//输出端口
|
long propeller,rudder,content;//校验序列、读取内容和解码内容
|
||||||
long check_array[7],content,after_decode;//校验序列、读取内容和解码内容
|
|
||||||
const byte addresses[6] = "00001"; //创建一个数组,建立接收机地址,或者说两个模块将用于通信的“管道”
|
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<add;j--,i++){
|
|
||||||
sum+=check_array[i]*j;
|
|
||||||
}
|
|
||||||
if(sum%10==check_array[4])
|
|
||||||
return -1;//代表校验失败
|
|
||||||
return x%10000;
|
|
||||||
}
|
|
||||||
void setup()
|
void setup()
|
||||||
{
|
{
|
||||||
|
|
||||||
Serial.begin(9600);
|
Serial.begin(9600);
|
||||||
MyServo.attach(4);
|
MyServo.attach(4);
|
||||||
RidaServo.attach(5);
|
RidaServo.attach(5);
|
||||||
|
@ -58,24 +43,25 @@ void setup()
|
||||||
radio.openReadingPipe(1,addresses); //接收通道,
|
radio.openReadingPipe(1,addresses); //接收通道,
|
||||||
radio.openWritingPipe(addresses);//发送通道
|
radio.openWritingPipe(addresses);//发送通道
|
||||||
radio.setPALevel(RF24_PA_HIGH); //设置功率放大器级别,将其设置为最小值
|
radio.setPALevel(RF24_PA_HIGH); //设置功率放大器级别,将其设置为最小值
|
||||||
//radio.setChannel(0);
|
|
||||||
delay(1000);
|
delay(1000);
|
||||||
|
MyServo.write(45);
|
||||||
|
}
|
||||||
|
long confirm(long x){
|
||||||
|
if(x>=1000000&&x<=2000000)return x%1000000;
|
||||||
|
else return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
void loop() {
|
void loop() {
|
||||||
//Serial.println(sizeof(long));
|
|
||||||
analogWrite(PWM,0);
|
|
||||||
radio.startListening();
|
radio.startListening();
|
||||||
// Serial.println("start listening!");
|
// Serial.println("start listening!");
|
||||||
|
/*if(angle<=0){
|
||||||
|
|
||||||
if(angle<=0){
|
|
||||||
dAngle=5;
|
dAngle=5;
|
||||||
}
|
}
|
||||||
else if(angle>=180){
|
else if(angle>=180){
|
||||||
dAngle=-5;
|
dAngle=-5;
|
||||||
}
|
}
|
||||||
angle=angle+dAngle;
|
/*angle=angle+dAngle;
|
||||||
RidaServo.write(angle);
|
RidaServo.write(angle);
|
||||||
digitalWrite(TrigPin,LOW);
|
digitalWrite(TrigPin,LOW);
|
||||||
delayMicroseconds(8);
|
delayMicroseconds(8);
|
||||||
|
@ -88,74 +74,76 @@ void loop() {
|
||||||
}
|
}
|
||||||
if(dist/17<period){
|
if(dist/17<period){
|
||||||
delay(period-dist/17);
|
delay(period-dist/17);
|
||||||
}
|
} */
|
||||||
radio.startListening();
|
radio.startListening();
|
||||||
// Serial.println("start listening!");
|
// Serial.println("start listening!");
|
||||||
if(radio.available()>0){
|
if(radio.available()>0){
|
||||||
radio.read(&content,sizeof(content));
|
radio.read(&content,sizeof(content));
|
||||||
Serial.println("The content is");
|
Serial.println("The content is");
|
||||||
Serial.println(content);//调试
|
Serial.println(content);//调试
|
||||||
after_decode=decode_and_confirm(content);
|
content=confirm(content);
|
||||||
Serial.println(after_decode);
|
if(content!=-1){
|
||||||
if(after_decode!=-1&&content>=100000){
|
propeller=content%1000;
|
||||||
if(content<200000){
|
rudder=(content-propeller)/1000;
|
||||||
Serial.println("inputing propeller ");
|
if(propeller>550||propeller<449)
|
||||||
if(after_decode>600||after_decode<500){
|
{
|
||||||
if(after_decode>600){
|
if(propeller>550)
|
||||||
long zheng=after_decode;
|
{
|
||||||
|
long zheng=0.568*propeller-312.4;
|
||||||
digitalWrite(IN2,HIGH);
|
digitalWrite(IN2,HIGH);
|
||||||
digitalWrite(IN1,LOW);
|
digitalWrite(IN1,LOW);
|
||||||
analogWrite(PWM,zheng);
|
analogWrite(PWM,zheng);
|
||||||
delay(50);
|
z=zheng;
|
||||||
z=zheng-512;
|
|
||||||
Serial.println("forward propelling");
|
Serial.println("forward propelling");
|
||||||
Serial.println(z);
|
Serial.println(z);
|
||||||
}
|
}
|
||||||
else if(after_decode<500){
|
else if(propeller<449)
|
||||||
long fan=1100-after_decode;
|
{
|
||||||
|
long fan=255-0.568*propeller;
|
||||||
digitalWrite(IN1,HIGH);
|
digitalWrite(IN1,HIGH);
|
||||||
digitalWrite(IN2,LOW);
|
digitalWrite(IN2,LOW);
|
||||||
analogWrite(PWM,fan);
|
analogWrite(PWM,fan);
|
||||||
delay(50);
|
z=fan;
|
||||||
z=fan-512;
|
|
||||||
Serial.println("back propelling");
|
Serial.println("back propelling");
|
||||||
Serial.println(z);
|
Serial.println(z);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else if(after_decode<=600||after_decode>=500){
|
else if(propeller<=550||propeller>=449)
|
||||||
|
{
|
||||||
z=0;
|
z=0;
|
||||||
|
analogWrite(PWM,0);
|
||||||
Serial.println("stop propelling");
|
Serial.println("stop propelling");
|
||||||
Serial.println(z);
|
Serial.println(z);
|
||||||
}
|
}
|
||||||
}
|
|
||||||
else{
|
|
||||||
Serial.println("inputing ruddering");
|
Serial.println("inputing ruddering");
|
||||||
if(after_decode>600||after_decode<500){
|
if(rudder>550||rudder<449)
|
||||||
if(after_decode<500){
|
{
|
||||||
long fan1=0.18*after_decode;
|
if(rudder<449)
|
||||||
MyServo.write(fan1/2);
|
{
|
||||||
delay(150);
|
long fan1=0.1*rudder;
|
||||||
dj=fan1-90;
|
MyServo.write(fan1);
|
||||||
Serial.println("turn left");
|
Serial.println("turn left");
|
||||||
|
dj=2*fan1-90;
|
||||||
Serial.println(dj);
|
Serial.println(dj);
|
||||||
}
|
}
|
||||||
else if(after_decode>600){
|
else if(rudder>550)
|
||||||
long zheng1=0.22*after_decode-42;
|
{
|
||||||
MyServo.write(zheng1/2);
|
long zheng1=0.1*rudder-10.122;
|
||||||
delay(150);
|
MyServo.write(zheng1);
|
||||||
dj=zheng1-90;
|
dj=2*zheng1-90;
|
||||||
Serial.println("turn right");
|
Serial.println("turn right");
|
||||||
Serial.println(dj);
|
Serial.println(dj);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else if(after_decode<=600||after_decode>=500){
|
else if(rudder<=550||rudder>=449)
|
||||||
|
{
|
||||||
dj=0;
|
dj=0;
|
||||||
|
MyServo.write(45);
|
||||||
Serial.println("stop turing");
|
Serial.println("stop turing");
|
||||||
Serial.println(dj);
|
Serial.println(dj);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
// radio.stopListening();
|
// radio.stopListening();
|
||||||
//radio.write(&dist,sizeof(dist));
|
//radio.write(&dist,sizeof(dist));
|
||||||
// radio.write(&dj,sizeof(dj));
|
// radio.write(&dj,sizeof(dj));
|
||||||
|
|
Loading…
Reference in New Issue