so, i build an rcx robot with 2 motors for going back or forward and one for direction (just like a real car). it works well so far, i can move it in all direction and ways i want with the function "fahren" (see code below).
however i now want the robot to be able to navigate, like back to the starting point or to koordinates. the developement is not finished, for now i only have a function to always save the position and the direction the robot is. starting at position (0|0) and direction (0|1), these informations are saved in two global arrays, k[0] for x of the position, k[1] for y of the position and d[0] and d[1] for x and y of the direction. this function is called "pos" and is called inside the "fahren" function.
i measured that the radius when it goes a circle is 5 times as big, as the way it goes in one second, so a whole circle takes about 15 seconds. so in one second it makes about 24 degrees of a circle. thats only to explain why i wrote winkel=24*t (winkel = angel and t = time in seconds)
i can compile and download it and it works well when i let the robot only go straightforward but when i tell it to turn it just beebs and says
"006^7 5" on the display. i dont know what should be wrong with my code except that i'm sure thats its not most elegant way for the requestet abilities. maybe someone of you has an idea where i made the mistake.
thanks in advance, here's the code:
- Code: Select all
import josx.platform.rcx.*;
public class mind6
{
static double[] k = {0,0} ; //k für koordinaten
static double[] d = {0,1} ; // d für direction
public static void set_power (int p) // p=power, 1-7
{
Motor.A.setPower(p);
Motor.B.setPower(p);
Motor.C.setPower(p);
}
public static void pos (int v, int r, double t) throws Exception {
t=t/1000;
double lang=1;
double ymitte=0;
double xmitte=0;
double radius=2.5;
double winkel=24*t;
lang=Math.pow((d[0]+d[1]), 0.5);
d[0]=d[0]/lang;
d[1]=d[1]/lang;
if (t>0 & v>0){
if (v==2){
d[0]=-d[0];
d[1]=-d[1];
}
if(r==2){
k[0]=k[0]+t*d[0];
k[1]=k[1]+t*d[1];
}
if(r==3){
xmitte=k[0]+t*d[1]; //rechts
ymitte=k[1]+t*(-d[0]);
k[0]= xmitte-(Math.cos (Math.toRadians(winkel))*radius);
k[1]= ymitte-(Math.sin (Math.toRadians(winkel))*radius);
d[0]=k[1]-ymitte; //rechts
d[1]=-k[0]-xmitte;
lang=Math.pow((d[0]+d[1]), 0.5);
d[0]=d[0]/lang;
d[1]=d[1]/lang;
}
if(r==1){
xmitte=k[0]+t*-(d[1]); //links
ymitte=k[1]+t*d[0];
k[0]= xmitte-(Math.cos (Math.toRadians(winkel))*radius);
k[1]= ymitte-(Math.sin (Math.toRadians(winkel))*radius);
d[0]=-k[1]-ymitte; //links
d[1]=k[0]-xmitte;
lang=Math.pow((d[0]+d[1]), 0.5);
d[0]=d[0]/lang;
d[1]=d[1]/lang;
}
if (v==2){
d[0]=-d[0];
d[1]=-d[1];
}
}
}
public static int fahren (int v, int r, int t, int s) throws Exception
{
pos(v, r, t);
int z=920;
int z1=2*z+200;
int z2=z1;
if(r==s) //grade
{
if(v==1){Motor.A.backward();Motor.B.backward();}
if(v==2){Motor.A.forward(); Motor.B.forward();}
Thread.sleep (t);
Motor.A.stop(); Motor.B.stop();
}
else if (s>r)
{ if(s-r==1) // halb links lenken
{
if(v==1){Motor.A.backward();Motor.B.backward();}
if(v==2){Motor.A.forward(); Motor.B.forward();}
Motor.C.backward();
if (t<z) { Thread.sleep (t); Motor.A.stop(); Motor.B.stop(); z=z-t;}
Thread.sleep (z);
Motor.C.stop();
if (t>z) { Thread.sleep (t-z); }
Motor.A.stop(); Motor.B.stop();
} else //ganz links lenken
{
if(v==1){Motor.A.backward();Motor.B.backward();}
if(v==2){Motor.A.forward(); Motor.B.forward();}
Motor.C.backward();
if (t<z1) { Thread.sleep (t); Motor.A.stop(); Motor.B.stop(); z2=z1-t;}
Thread.sleep (z2);
Motor.C.stop();
if (t>z1) { Thread.sleep (t-z1); }
Motor.A.stop(); Motor.B.stop();
}
}
else if(r-s==1) // halb rechts lenken
{
if(v==1){Motor.A.backward();Motor.B.backward();}
if(v==2){Motor.A.forward(); Motor.B.forward();}
Motor.C.forward();
if (t<z) { Thread.sleep (t); Motor.A.stop(); Motor.B.stop(); z=z-t;}
Thread.sleep (z);
Motor.C.stop();
if (t>z) { Thread.sleep (t-z); }
Motor.A.stop(); Motor.B.stop();
}
else //ganz rechts lenken
{
if(v==1){Motor.A.backward();Motor.B.backward();}
if(v==2){Motor.A.forward(); Motor.B.forward();}
Motor.C.forward();
if (t<z1) { Thread.sleep (t); Motor.A.stop(); Motor.B.stop(); z2=z1-t;}
Thread.sleep (z2);
Motor.C.stop();
if (t>z1) { Thread.sleep (t-z1); }
Motor.A.stop(); Motor.B.stop();
}
return r;
}
public static void main (String[] args) throws Exception
{
/* int fahren ( v r t s)
v: vorne(1) / hinten(2) / stehen(0)
r: links(1) / grade(2) / rechts(3)
t: Zeit in ms
s: status
returns: status
*/
int status;
status=2;
set_power(7);
try
{
Thread.sleep (1000);
status=fahren (0, 3 ,0, status);
status=fahren (1, 3 ,3000, status); status=fahren (0, 2 ,0, status);
status=fahren (1, 2 ,7000, status);
status=fahren (0, 2 ,0, status);
} catch(Exception e){ }
}
}