|
|
本帖最后由 songjiahaonanna 于 2015-8-13 15:01 编辑
用的 Mini,但是不知道怎么程序总是自动复位，我在 set up 里写了个串口 Serial.println("restart");
程序运行过程中串口输出总会显示板子复位了，不知道什么原因。。。
[pre lang="arduino" line="1" file="sketch_jun14a"]//#include <FlexiTimer2.h>
//#include <TimerOne.h>
//#include <avr/wdt.h>
#include <MsTimer2.h>
#include <Servo.h>
Servo myservo;
float angle;
volatile int A;
volatile int B;
volatile int E;
//const int green=4;
//const int yellow=2;
//const int blue=3;
#define green 4
#define yellow 2
#define blue 3
volatile int K1,K2,K3;
int FLAG;
float X,M1,M2,M3;
volatile int N,nn;
volatile char AD_V;
volatile int Count,DD,EE,FF,GG,Current_Detection,Restriction,WDT_reset;
volatile int Delay_300ms,Delay_200ms;
volatile int flag_1,flag_2,flag_3,Motor_2_Current_Detection;
float error_I,deviation,Angle_PD;
volatile float xianzhi_1,xianzhi_2,xianzhi_3;
int Angle_PD_OUT;
float error,Now_error,Last_error,Pre_Last_error;
#define Angle_P 50
#define Angle_I 0
#define Angle_D 0
void Timer()
{
Count++;
if(Count==20)
{
Count=0;
}
EE++;
if(EE==100)
{
EE=0;
K3=digitalRead(2);
K1=digitalRead(4);
K2=digitalRead(3);
// WDT_reset=1;
// Motor_Current_Detection();
}
FF++;
if(FF==300)
{
FF=0;
Delay_300ms=1;
// xianwei();
}
GG++;
if(GG==200)
{
GG=0;
Delay_200ms=1;
Restriction=1;
Current_Detection=1;
}
}
void AD(int AD_V)
{
int V1=analogRead(AD_V);
angle=V1*(5.0/1023.0);
// angle=map(V1,0,1023,0,360);
// Serial.println("angle");
// Serial.println(angle);
}
void AD_1()
{
int V2=analogRead(A4);
xianzhi_1=V2*(5.0/1023.0);
// M1=xianzhi_1;
// Serial.println("AD_1");
// Serial.println(xianzhi_1);
}
void AD_2()
{
int V2=analogRead(A6);
xianzhi_2=V2*(5.0/1023.0);
// Serial.println("AD_2");
// Serial.println(xianzhi_2);
}
void AD_3()
{
int V2=analogRead(A5);
xianzhi_3=V2*(5.0/1023.0);
// Serial.println("AD_3");
// Serial.println(xianzhi_3);
}
void PWM1(int DD)
{
if(Count<DD)
digitalWrite(A, HIGH);
else
digitalWrite(A, LOW);
}
void PWM2(int DD)
{
if(Count<DD)
digitalWrite(B, HIGH);
else
digitalWrite(B, LOW);
}
void Motor_forward_back(int duty)
{
digitalWrite(E, HIGH);
if(duty>0)
{
PWM1(duty);
digitalWrite(B,LOW);
}
else
{
PWM2(-duty);
digitalWrite(A,LOW);
}
}
//
void Angle_control(float jiaodu)
{
int V1=analogRead(AD_V);
angle=V1*(5.0/1023.0);
// Serial.println(angle);
error=jiaodu-angle;
error_I+=error;
Now_error=error;
deviation=Now_error-Last_error;
Angle_PD=error*Angle_P+error_I*Angle_I+deviation*Angle_D;
Last_error=Now_error;
Angle_PD/=2;
if(Angle_PD>19)
{
Angle_PD=19;
}
if(Angle_PD<-19)
{
Angle_PD=-19;
}
Motor_forward_back((int) Angle_PD);
}
void Change(int channel,float jiaodu)
{
X=jiaodu;
N=channel;
switch(channel)
{
case 1:
{
A=9;
B=11;
E=10;
AD_V=A1;
break;
}
case 2:
{
A=6;
B=7;
E=8;
AD_V=A2;
break;
}
case 3:
{
A=13;
B=A0;
E=12;
AD_V=A3;
break;
}
default:
break;
}
pinMode(A,OUTPUT);
pinMode(B,OUTPUT);
pinMode(E,OUTPUT);
digitalWrite(E, HIGH);
// AD(AD_V);
Angle_control(X);
}
void Motor_brake_1()
{
digitalWrite(9,LOW);
digitalWrite(11,LOW);
digitalWrite(10,LOW);
}
void Motor_brake_2()
{
digitalWrite(6,LOW);
digitalWrite(7,LOW);
digitalWrite(8,LOW);
}
void Motor_brake_3()
{
digitalWrite(13,LOW);
digitalWrite(A0,LOW);
digitalWrite(12,LOW);
}
void zhengzhuan()
{
digitalWrite(9,HIGH);
digitalWrite(11,LOW);
digitalWrite(10, HIGH);
digitalWrite(6,HIGH);
digitalWrite(7,LOW);
digitalWrite(8, HIGH);
digitalWrite(13,HIGH);
digitalWrite(A0,LOW);
digitalWrite(12, HIGH);
}
void Motor_Current_Detection()
{
AD_1();
AD_2();
AD_3();
if(xianzhi_1>1)
{
if(Delay_300ms==1)
{
if(xianzhi_1>1)
{
// flag_1=1;
Motor_brake_1();
Serial.println("Motor_1_STOP");
}
Delay_300ms=0;
}
}
if(xianzhi_2>1)
{
if(Delay_300ms==1)
{
if(xianzhi_2>1)
{
Motor_2_Current_Detection++;
if(Motor_2_Current_Detection==3)
{
Motor_brake_2();
Serial.println("Motor_2_STOP");
Motor_2_Current_Detection=0;
}
}
Delay_300ms=0;
}
}
if(xianzhi_3>1)
{
if(Delay_300ms==1)
{
if(xianzhi_3>1)
{
// flag_3=1;
Serial.println("Motor_3_STOP");
Motor_brake_3();
}
Delay_300ms=0;
}
}
}
void Servo_Detect()
{
switch(nn)
{
case 0: myservo.writeMicroseconds(1080);
break;
case 1: myservo.writeMicroseconds(950);
break;
case 2: myservo.writeMicroseconds(1410);
break;
case 3: nn=0;
break;
default:
break;
}
}
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
MsTimer2::set(1,Timer);
MsTimer2::start();
myservo.attach(5);
// myservo.writeMicroseconds(1080);
// pinMode(A,OUTPUT);
// pinMode(B,OUTPUT);
// pinMode(E,OUTPUT);
pinMode(12, OUTPUT);
pinMode(13,OUTPUT);
pinMode(A0,OUTPUT);
pinMode(11,OUTPUT);
pinMode(10,OUTPUT);
pinMode(9,OUTPUT);
pinMode(6,OUTPUT);
pinMode(7,OUTPUT);
pinMode(8,OUTPUT);
pinMode(yellow, INPUT);
pinMode(green, INPUT);
pinMode(blue, INPUT);
pinMode(yellow,INPUT_PULLUP);
pinMode(green,INPUT_PULLUP);
pinMode(blue,INPUT_PULLUP);
// wdt_enable(WDTO_2S);
Serial.println("restart");
}
void loop()
{
// wdt_reset();
if(Current_Detection==1)
{
Motor_Current_Detection();
Current_Detection=0;
}
if(K1==0)
{
if(Delay_200ms==1)
{
if(K1==0)
{
zhengzhuan();
K1=1;
Serial.println("zhengzhuan");
}
}
Delay_200ms=0;
}
if(K2==0)
{
if(Delay_200ms==1)
{
if(K2==0)
{
int i;
for(i=0;i<200;i++)
{
Change(1,1.9);
Change(2,1.9);
Change(3,2.1);
// Serial.println("jiaodu");
}
K2=1;
}
Delay_200ms=0;
}
}
if(Delay_200ms==1)
{
if(K3==0)
{
if(Delay_300ms==1)
{
if(K3==0)
{
nn++;
}
Delay_300ms=0;
}
}
Delay_200ms=0;
}
Servo_Detect();
}
[/code]
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看，没有帐号？注册
x
|
发表于 2015-8-12 14:45:25
楼主
