Renjian Hao
Renjian Hao
Dependencies: L3G4200D L3GD20 LSM303DLHC LSM303DLM PwmIn Servo mbed
Fork of
Fish_2014Fall
by 
Zhan Tu
Revision 5:a5f0395d2fa4, committed 2015-08-13
- Comitter:
- RenjianHao
- Date:
- Thu Aug 13 18:37:15 2015 +0000
- Parent:
- 4:c93e1ecd3359
- Commit message:
- Renjian Hao2015/8/13
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Jul 15 20:13:09 2015 +0000
+++ b/main.cpp Thu Aug 13 18:37:15 2015 +0000
@@ -15,7 +15,9 @@
#define TS 10 // sample time [ms]
#define SIMULATION_TIME 10 // simulation time [s]
-
+#define Left 1
+#define Right 0
+#define Lost 555
//serial
Serial BB(p13, p14); // tx, rx
@@ -32,6 +34,8 @@
bool directionB;
bool directionC;
bool directionE;
+bool Target;
+bool direction;
float speedA;
float speedB;
@@ -43,8 +47,10 @@
float AngleTail=0.08;
float Tailbias=0.0;
float AngleTail_stand_bias=0.0;
+float dirE=1.0;
+float bias=0.0;
-long int FA=1000000,FB=1000000,FE=4,CountA=0,CountB=0,CountE=0,CountT=0;
+long int FA=1000000,FB=1000000,FE=40,CountA=0,CountB=0,CountE=0,CountT=0;
int flag;
int x,y; // Image coordinate
@@ -88,13 +94,13 @@
if(directionA)
{
//speedA=0.4;
- PWMA.pulsewidth(period_pwm*(speedA*1.5));
+ PWMA.pulsewidth(period_pwm*(speedA*1));
//PWMA.pulsewidth(period_pwm*(0.6));
}
else
{
//speedA=0.2;
- PWMA.pulsewidth(period_pwm*speedA*1.5);
+ PWMA.pulsewidth(period_pwm*speedA*1);
}
//PWMA.pulsewidth(period_pwm*speedA);
@@ -117,13 +123,13 @@
if(!directionB)
{
//speedB=0.4;
- PWMB.pulsewidth(period_pwm*(speedB*1.5));
+ PWMB.pulsewidth(period_pwm*(speedB*1));
//PWMB.pulsewidth(period_pwm*(0.6));
}
else
{
//speedB=0.2;
- PWMB.pulsewidth(period_pwm*speedB*1.5);
+ PWMB.pulsewidth(period_pwm*speedB*1);//1.5 7-28
}
//PWMB.pulsewidth(period_pwm*speedB);
@@ -200,8 +206,8 @@
//Actuator & servo setup
- period_pwm_ac=0.0020; //500hz
- period_pwm=0.020; //20ms
+ period_pwm_ac=0.010; //500hz/5khz
+ period_pwm=0.020; //10ms
PWMA.period(period_pwm_ac);
PWMB.period(period_pwm_ac);
PWMC.period(period_pwm); // servo requires a 20ms period
@@ -227,7 +233,7 @@
// pc.printf(str);
//Talk to BBB
-
+
int i;
if(BB.readable()>0)
{
@@ -241,11 +247,12 @@
token = strtok(NULL, ",");
y = atoi(token); //100-340(height_240); No target:555
- //pc.printf("%d \r \n",y);
+ //pc.printf("%d",y);
//pc.printf("\n");
//pc.printf(str);
}
}
+
/*
if(y<300&&y>100)
{
@@ -257,8 +264,38 @@
Tailbias=0;
}
*/
- FA=20;//5;
- FB=20;//5;
+ FA=5;//5;
+ FB=5;//5;
+
+ //speedA=0.0;
+ //speedB=0.95;
+ /*
+ //Tailbias=0;
+ if(CountT<=50)
+ {
+ //Tailbias=0;//(200-160)*0.0006;
+ CountT++;
+ }
+ else if(CountT>50&&CountT<=100)
+ {
+ Tail_increasement=speedA*0.0012;
+ CountT++;
+ }
+ else
+ {
+ CountT=0;
+ }
+
+ Tailbias=Tailbias-Tail_increasement;//(y-200)*0.0006;
+ if(Tailbias>0.03)
+ {
+ Tailbias=0.03;
+ }
+ if(Tailbias<-0.03)
+ {
+ Tailbias=-0.03;
+ }
+ */
/*
if(CountT>300&&CountT<=600)
{
@@ -279,38 +316,61 @@
CountT=0;
}
*/
-
+ /*Tracking demo*/
+
if(y<350&&y>200)
{
- speedA=(y-200)*0.006;
- Tail_increasement=speedA*0.0012;
- Tailbias=Tailbias-Tail_increasement;//(y-200)*0.0006;
+ speedA=(y-200)*0.005+0.1;
+ //Tail_increasement=speedA*0.0006;
+ //Tailbias=Tailbias-Tail_increasement;//(y-200)*0.0006;
speedB=0.0;
+ Target=1;
+ bias=-0.01;//(200-y)*0.0001;
+ direction=Left;
}
else if (y>50&&y<=200)
{
- speedB=(200-y)*0.006;
- Tail_increasement=speedB*0.0012;
- Tailbias=Tailbias+Tail_increasement;//0.0;//(y-200)*0.0006;
+ speedB=(200-y)*0.005+0.1;
+ //Tail_increasement=speedB*0.0006;
+ //Tailbias=Tailbias+Tail_increasement;//0.0;//(y-200)*0.0006;
speedA=0.0;
+ Target=1;
+ bias=0.01;//(200-y)*0.0001;
+ direction=Right;
}
else
{
speedA=0.0;
speedB=0.0;
//Tailbias=0.0;
- }
-
- if(Tailbias>0.02)
- {
- Tailbias=0.02;
+ Target=0;
+ direction=Lost;
}
- if(Tailbias<-0.02)
+ bias=0;
+ if(Target)
+ {
+ if(Tailbias>=(0.008+bias))
+ {
+ dirE=-1.0;
+ }
+ else if(Tailbias<=(-0.008+bias))
+ {
+ dirE=1.0;
+ }
+ Tailbias=Tailbias+0.0016*dirE;
+ }
+
+ /*
+ if(Tailbias>0.03)
{
- Tailbias=-0.02;
+ Tailbias=0.03;
}
-
+ if(Tailbias<-0.03)
+ {
+ Tailbias=-0.03;
+ }
+ */
/*
if(y>210&&y<400)
@@ -411,7 +471,7 @@
{
CountE++;
}
- */
+ */
//6/29/2015 Tail
if(x<300&&x>100)
{
@@ -455,9 +515,12 @@
//moveD();
moveA();
+ //
+
+ //directionB=0;
moveB();
- moveE();
+ //moveE();
wait(0.03);