這是在第一代上的更新 由遙控車變為可以走到指定點。 float spaceD=182.12;//步距
int Xinstance1=0,Yinstance1=0;//初始化坐标點
float stept=0;//存儲電機步數
int angle=0;//存儲角度變量
float tanangle=90;
int Xstept_Pin=54;//序列槽連接配接
float ang[46]={0,0.01,0.03,0.05,0.06,0.08,0.10,0.12,0.14,0.15,0.17,0.19,0.21,0.23,0.24,0.26,0.28,0.30,0.32,0.34,0.36,0.38,0.40,0.42,0.44,0.46,0.48,0.50,0.53,0.55,0.57,0.60,0.62,0.64,0.67,0.70,0.72,0.75,0.78,0.80,0.83,0.86,0.90,0.93,0.96,1.00};
int XDir_Pin=55;
int XEN_Pin=38;
int Ystept_Pin=60;
int YDir_Pin=61;
int YEN_Pin=56;
int Zstept_Pin=46;
int ZDir_Pin=48;
int ZEN_Pin=62;
float angl=0;
float step;
float x=0;//存儲X方向地距離
float y=0;//存儲Y方向地距離
char a[2][4];//存儲輸入資料
void setup() {
// put your setup code here, to run once:
Serial .begin(9600);
pinMode(Xstept_Pin,OUTPUT);
pinMode(XDir_Pin,OUTPUT);
pinMode(XEN_Pin,OUTPUT);
pinMode(Ystept_Pin,OUTPUT);
pinMode(YDir_Pin,OUTPUT);
pinMode(YEN_Pin,OUTPUT);
pinMode(Zstept_Pin,OUTPUT);
pinMode(ZDir_Pin,OUTPUT);
pinMode(ZEN_Pin,OUTPUT);
digitalWrite(XEN_Pin,LOW);
digitalWrite(YEN_Pin,LOW);
digitalWrite(ZEN_Pin,LOW);//初始化
Serial.println("please input the site of x and y in order");
} void loop()
{
float broder[2]={0,0};//新得到的坐标
int h[4];//存儲實數部分
int b[2];//存儲小數部分
int X,Y,i=0;
char p;
float k=1.0;
char a[2][7];
if(Serial.available())
{
for(int j=0;j<2;j++)
{
for(i=0;i<7;i++)//讀入7位地一個字元 4位整數,2位小數
{
char p=Serial.read();//登出測試,能成功讀入序列槽數值
a[j][i]=p;
Serial.println(a[j][i]);
delay(300);
}
delay(3000);//等待使有足夠的時間讀入下一組資料
}//讀入了一X、Y地坐标值
if(a[0][0]!='-')//确定輸入坐标地正負以保證可以合理地把這組數字分離出來
{
for(i=0;i<4;i++)
{
if(a[0][i]>='0'&&a[0][i]<='9')//将字元型轉換為數字型
{
h[i]=a[0][i]-'0';//整數部分
}
else
Serial.println("input erorn");
delay(200); //可以成功轉化出X坐标
}
for(i=5;i<7;i++)
{
if(a[0][i]>='0'&&a[0][i]<='9')//将字元型轉換為數字型
{
b[i-5]=a[0][i]-'0';//2位小數部分
}
}
for(i=3;i>=0;i--)
{
broder[0]=h[i]*k+broder[0];//将個位數字拼接
k=k*10;
}
k=1;
for(i=0;i<2;i++)
{
k=k*0.1;
broder[0]=b[i]*k+broder[0];
}
Serial.print("broder[0]=");
Serial.println(broder[0]);//X方向的步伐
}
else
{
Serial.print("mmp");
for(i=1;i<4;i++)
{
if(a[0][i]>='0'&&a[0][i]<='9')//将字元型轉換為數字型
{
h[i-1]=a[0][i]-'0';//整數部分 調試正确
Serial.print("i=");
Serial.println(i);
Serial.print("h=");
Serial.println(h[i-1]);
}
else
Serial.println("input erorn");
delay(200); //可以成功轉化出X坐标
}
for(i=5;i<7;i++)
{
if(a[0][i]>='0'&&a[0][i]<='9')//将字元型轉換為數字型
{
b[i-5]=a[0][i]-'0';//2位小數部分
Serial.println(b[i-5]);
}
}
k=1;
for(i=2;i>=0;i--)
{
broder[0]=h[i]*k+broder[0];//将個位數字拼接
k=k*10;
}
k=1;
for(i=0;i<2;i++)
{
k=k*0.1;
broder[0]=b[i]*k+broder[0];
}
broder[0]= 0-broder[0];
Serial.print("broder[0]=");
Serial.println(broder[0]);//X方向的步伐
}
if(a[1][0]!='-')//确定輸入坐标地正負以保證可以合理地把這組數字分離出來
{
for(i=0;i<4;i++)
{
if(a[1][i]>='0'&&a[1][i]<='9')//将字元型轉換為數字型
{
h[i]=a[1][i]-'0';//整數部分 }
else
Serial.println("input erorn");
delay(200); //可以成功轉化出X坐标
}
for(i=5;i<7;i++)
{
if(a[1][i]>='0'&&a[1][i]<='9')//将字元型轉換為數字型
{
b[i-5]=a[1][i]-'0';//2位小數部分
//調試成功
}
}
k=1;
for(i=3;i>=0;i--)
{
broder[1]=h[i]*k+broder[1];//将個位數字拼接
k=k*10;
}
k=1;
for(i=0;i<2;i++)
{
k=k*0.1;
broder[1]=b[i]*k+broder[1];
}
Serial.print("broder[1]=");
Serial.println(broder[1]);//Y方向的步伐
}
else
{
for(i=1;i<4;i++)
{
if(a[0][i]>='0'&&a[0][i]<='9')//将字元型轉換為數字型
{
h[i-1]=a[1][i]-'0';//整數部分 調試正确
Serial.print("i=");
Serial.println(i);
Serial.print("h=");
Serial.println(h[i-1]);
}
else
Serial.println("input erorn");
delay(200); //可以成功轉化出X坐标
}
for(i=5;i<7;i++)
{
if(a[1][i]>='0'&&a[1][i]<='9')//将字元型轉換為數字型
{
b[i-5]=a[1][i]-'0';//2位小數部分
Serial.println(b[i-5]);
}
}
k=1;
for(i=2;i>=0;i--)
{
broder[1]=h[i]*k+broder[1];//将個位數字拼接
k=k*10;
}
k=1;
for(i=0;i<2;i++)
{
k=k*0.1;
broder[1]=b[i]*k+broder[1];
}
broder[1]= 0-broder[1];
Serial2.print("broder[1]=");
Serial2.println(broder[1]);//Y方向的步伐
}
//上面地代碼已經完全地把輸入進來地坐标存儲到了broder數組中
x=broder[0]-Xinstance1;
y=broder[1]-Yinstance1;//計算距離差 每次都認為小車地中心是坐标圓心 Y輪所正對地方向是Y軸正方向
//上面代碼已測試成功
Serial.print("x=");
Serial.println(x);
Serial.print("y=");
Serial.println(y);
if(x>0&&y>0)
{
digitalWrite(XDir_Pin,LOW);
digitalWrite(YDir_Pin,LOW);
digitalWrite(ZDir_Pin,LOW);
}
if(x<0&&y>0)
{
digitalWrite(XDir_Pin,HIGH);
digitalWrite(YDir_Pin,HIGH);
digitalWrite(ZDir_Pin,HIGH);
x=-x;
}
if(x<0&&y<0)
{
digitalWrite(XDir_Pin,HIGH);
digitalWrite(YDir_Pin,HIGH);
digitalWrite(ZDir_Pin,HIGH);
angl=90;
x=-x;
y=-y;
}
if(x>0&&y<0)
{
digitalWrite(XDir_Pin,LOW);
digitalWrite(YDir_Pin,LOW);
digitalWrite(ZDir_Pin,LOW);
angl=90;
y=-y;
}//區分了象限
int f=0;//做标志 辨別
if(y>x)
{
tanangle=y/x;
f=-1;
}
else
{
tanangle=x/y;
f=1;
}
tanangle=int(tanangle*100);
tanangle=tanangle*0.01;//計算出樂轉動角度地正切值
for(int i=0;i<46;i++)
{
if(ang[i]==tanangle)
{
angle=i;
break;
}
}
//上面問題:1、缺乏和原資料地對比 2、沒辦法解決負數地坐标 3、要把轉動函數寫到上面去
//經測試計算得 3200是一周 1個脈沖0.1125度
if(f==1)
angle=90-angle;
angle=angle+angl;//寫到此處發現問題:小車轉動是Y軸轉過地角度 還是小車中心做圓心是轉過地角度 135
step=(angle*3.14*135)/180;
step=(step/spaceD)*360*8.888;//得出轉動所需脈沖數
for(int i=0;i<=step;i++)
{ digitalWrite(Xstept_Pin,HIGH);
digitalWrite(Zstept_Pin,HIGH);
digitalWrite(Ystept_Pin,HIGH);
delayMicroseconds(200);
digitalWrite(Xstept_Pin,LOW);
digitalWrite(Zstept_Pin,LOW);
digitalWrite(Ystept_Pin,LOW);
delayMicroseconds(200);
}
delay(300);
stept=int((sqrt(pow(x,2)+pow(y,2)))/(spaceD*cos(30)));//求出了脈沖數
Serial.println("stept=");
Serial.println(stept);
digitalWrite(ZDir_Pin,HIGH);
digitalWrite(XDir_Pin,LOW);
for(int i=0;i<=stept*3200;i++)
{ digitalWrite(Zstept_Pin,HIGH);
digitalWrite(Xstept_Pin,HIGH);
delayMicroseconds(200);
digitalWrite(Zstept_Pin,LOW);
digitalWrite(Xstept_Pin,LOW);
delayMicroseconds(200);
}//向前行
}
}