Can use SERIN to read PIN 2 of Javelin Stamp?

StereoPonyz

Hi peter, i am testing out Angle calculation using the StarGazer. However, the computation is always zero. May·i know is it that trigonometric function cannot take in floating numbers of xpos and ypos?

=====================================================================================

import stamp.core.*;
import stamp.math.*;
import stamp.util.text.*;


public class StargazerNewMove {
· static Uart myUart = new Uart(Uart.dirReceive,CPU.pin2,Uart.invert,Uart.speed9600,Uart.stop1);

· static char[noparse]/noparse responseBuf = new char[noparse][[/noparse]128];
· static int[noparse]/noparse scanvalue = new int[noparse][[/noparse]1];

/*
· 10a. Format of Received Data
· The format of data received from StarGazer for the command ~#CalcStart` are as follows:
······ F
·· ~ ^ I iiii|±aaaa.aa|±xxxx.xx|±yyyy.yy|zzzz.zz `
······ Z
·· ^· Means the result data
·· F· Indicates the Map-Building Mode
·· I Indicates the Map Mode
·· Z Indicates the Height Calculation Mode
·· iiii The number of an ID
·· ±aaaa.aa Value of Angle (degrees; -180~+180)
·· ±xxxx.xx Position on X axis (cm)
·· ±yyyy.yy Position on Y axis (cm)
·· zzzz.zz· Height (cm)
*/

· static int mode;··· // 'F', 'I', 'Z'
· static int id;
· static int angle;·· // in 0.01 deg units (-18000 to +18000)
· static int xpos;··· // in 0.01 cm units
· static int ypos;··· // in 0.01 cm units
· static int height;· // in 0.01 cm units

· static int FAngle = 90; //Fixed the heading of robot at 90degrees
· static int x = 0;
· static int y = 0;

· static PWM pwmR = new PWM(CPU.pin12); // create right servo
· static PWM pwmL = new PWM(CPU.pin13); // create left servo

· static int abs(int value) {
··· return (value < 0) ? -value : value;
· }

//receive reponse from StarGazer
static void response() {
· char c;
· int k,num;
· while (true) {
··· k = 0;
··· do { //wait for ~
····· c = (char)myUart.receiveByte();
··· } while (c != '~');
··· responseBuf[noparse][[/noparse]k++] = c;
··· do { //receive until `
····· c = (char)myUart.receiveByte();
····· responseBuf[noparse][[/noparse]k++] = c;
··· } while (c != '`');
··· responseBuf[noparse][[/noparse]k] = 0; //closing null
··· num = 0;

··· for (int i=0; i<k; i++) { //check for 4 '|' => if there are no 4 '|' skip
····· if (responseBuf[noparse][[/noparse]i] == '|') num++;
··· }
··· if (num != 4) continue; //skip invalid message

··· //get mode
··· k = Format.bscanf(responseBuf,0,"~^%c",scanvalue);
··· mode = scanvalue[noparse][[/noparse]0] & 255;
··· //get id
··· k = Format.bscanf(responseBuf,k,"%d|",scanvalue);
··· id = scanvalue[noparse][[/noparse]0];
··· //get angle integer part
··· k = Format.bscanf(responseBuf,k,"%d.",scanvalue);
··· angle = scanvalue[noparse][[/noparse]0];
··· //get angle fractional part
··· k = Format.bscanf(responseBuf,k,"%d|",scanvalue);
··· angle = (angle*100);
··· if (angle < 0) angle -= scanvalue[noparse][[/noparse]0];
··· else angle += scanvalue[noparse][[/noparse]0];
··· //get xpos integer part
··· k = Format.bscanf(responseBuf,k,"%d.",scanvalue);
··· xpos = scanvalue[noparse][[/noparse]0];
··· //get xpos fractional part
··· k = Format.bscanf(responseBuf,k,"%d|",scanvalue);
··· xpos = (xpos*100);
··· if (xpos < 0) xpos -= scanvalue[noparse][[/noparse]0];
··· else xpos += scanvalue[noparse][[/noparse]0];
··· //get ypos integer part
··· k = Format.bscanf(responseBuf,k,"%d.",scanvalue);
··· ypos = scanvalue[noparse][[/noparse]0];
··· //get ypos fractional part
··· k = Format.bscanf(responseBuf,k,"%d|",scanvalue);
··· ypos = (ypos*100);
··· if (ypos < 0) ypos -= scanvalue[noparse][[/noparse]0];
··· else ypos += scanvalue[noparse][[/noparse]0];
··· //get LastValue integer part
··· k = Format.bscanf(responseBuf,k,"%d.",scanvalue);
··· height = scanvalue[noparse][[/noparse]0];
··· //get LastValue fractional part
··· k = Format.bscanf(responseBuf,k,"%d`",scanvalue);
··· height = (height*100);
··· if (height < 0) height -= scanvalue[noparse][[/noparse]0];
··· else height += scanvalue[noparse][[/noparse]0];
··· //print full response
··· Format.printf("received: %s\n",responseBuf);
··· break;
· }
}

static void PlusPlus() {
··· int AngletoTurn, AngletoFace;
··· int TestAngletoTurn;
··· AngletoTurn = Trig.arctan(xpos/ypos);
··· System.out.println("Amount of angle to turn: "+AngletoTurn);
··· AngletoFace = FAngle - AngletoTurn;
··· System.out.println("Angle e robot sd face: "+AngletoFace);
····· while (true) {
······· response();
······· TestAngletoTurn = Trig.arctan(xpos/ypos);
······· System.out.println("Amount of angle to turn: "+TestAngletoTurn);
··········· if ((angle >= (AngletoFace - 1)) && (angle <= (AngletoFace + 1)))
··········· {//stop turn;
············· return;
··········· }

··········· else
··········· {
············· turnClockwise();
··········· }
····· }
}

static void PlusMinus() {
··· System.out.println("+,-");
}

static void MinusPlus() {
··· System.out.println("-,+");
}

static void MinusMinus() {
··· System.out.println("-,-");
····· }

static void moveStraight() {
··· System.out.println("move straight");
··· pwmR.update ( 171, 2304 ) ;· //A-CW < 173 < CW
··· pwmL.update ( 175, 2304 ) ;· //move Straight
··· pwmR.start () ;
··· pwmL.start () ;
··· CPU.delay(100);
··· pwmR.stop () ;
··· pwmL.stop () ;
··· if (angle/100 > 91) turnAntiClockwise ();
················· }

static void turnClockwise() {
··· System.out.println("turnClockwise");
··· pwmR.update ( 175, 2304 ) ;· //A-CW < 173 < CW
··· pwmL.update ( 175, 2304 ) ;· //turn CW
··· pwmR.start () ;
··· pwmL.start () ;
··· CPU.delay(100);
··· pwmR.stop () ;
··· pwmL.stop () ;
················· }

static void turnAntiClockwise() {
··· System.out.println("turnAntiClockwise");
··· pwmR.update ( 171, 2304 ) ;· //A-CW < 173 < CW
··· pwmL.update ( 171, 2304 ) ;· //turn CW
··· pwmR.start () ;
··· pwmL.start () ;
··· CPU.delay(100);
··· pwmR.stop () ;
··· pwmL.stop () ;
················· }


static void main() {
··· Format.printf("StarGazer test program\n");
··· Format.printf("~^MapMode IDNum | Angle | X | Y | Height `");
//··· while (true) {
··· response();·· //get a full response, extract values and print response
··················· //recreate response output from extracted values, should be identical to response

··· if ((xpos - x > 0) && (ypos - y > 0))
··· {//· ++ scenario
········ PlusPlus ();
··· }
··· else if ((xpos - x > 0) && (ypos - y < 0))
··· {//· +- scenario
········ PlusMinus ();
··· }
··· else if ((xpos - x < 0) && (ypos - y > 0))
··· {//· -+ scenario
········ MinusPlus ();
··· }
··· else if ((xpos - x < 0) && (ypos - y < 0))
··· {//· -- scenario
········ MinusMinus ();
··· }


····· Format.printf("\nRecreate: ~^");
····· Format.printf("%c",(char)mode);
····· Format.printf("%d|",id);
····· if (angle >= 0) Format.printf("+");
····· Format.printf("%d.",angle/100);
····· Format.printf("%02d|",abs(angle)%100);
····· if (xpos >= 0) Format.printf("+");
····· Format.printf("%d.",xpos/100);
····· Format.printf("%02d|",abs(xpos)%100);
····· if (ypos >= 0) Format.printf("+");
····· Format.printf("%d.",ypos/100);
····· Format.printf("%02d|",abs(ypos)%100);
····· //if (LastValue >= 0) Format.printf("+");
····· Format.printf("%d.",height/100);
····· Format.printf("%02d`\n",abs(height)%100);
//··· }
· }
}

====================================================================================

Sorry, site cant let me upload my java codes

Peter Verkaik

You can't do
System.out.println("Amount of angle to turn: "+AngletoTurn);

Split it up like
System.out.print("Amount of angle to turn: ");
System.out.println(AngletoTurn);

or use
Format.printf("Amount of angle to turn: %d\n",AngletoTurn);

regards peter

Peter Verkaik

You'd better use Trig.atan2(ypos,xpos)

· /**
·· * Calculate arctan(y/x)
·· *
·· * @param y Vertical signed value
·· * @param x Horizontal signed value
·· * @return Angle in 0.1 degree units (-1799 to +1800)
·· */

rather than Trig.arctan(ypos/xpos)
· /**
·· * Calculate arctan(x)
·· *
·· * @param x Tangens value in 0.01 units (-32767 to +32767)
·· * @return Angle in 0.1 degree units (-900 to +900)
·· */


If you multiply the Trig.atan2(ypos,xpos) by 10, then the range is -17990 to +18000
which is the same range as your stargazer angle value (0.01 units)

regards peter

StereoPonyz

Hi Peter, my robot is jerking instead of turning slowly in a smooth motion.

=========================================
static void turnAntiClockwise() {
System.out.println("turnAntiClockwise");
pwmR.update ( 171, 2304 ) ; //A-CW < 173 < CW
pwmL.update ( 171, 2304 ) ; //turn CW
pwmR.start () ;
pwmL.start () ;
CPU.delay(100);
pwmR.stop () ;
pwmL.stop () ;
}
==========================================
The abv is the pluse count i used to set on the robot to move slowly. But i wanted the movement to be small, such that it move with every change of 1 to 2 degress, thats why i put the delay as 100. But setting the delay too short, the robot jerks instead of smooth motion.

StereoPonyz

Hi Peter, can i also check if i would need to make changes to my condition as it do not seems to execute it.

========================================================
static void PlusPlus() {
··· int AngletoTurn, AngletoFace;
··· int TestAngletoTurn;
··· AngletoTurn = (Trig.atan2(ypos,xpos))/10;
··· Format.printf("Amount of angle to turn: %d\n",AngletoTurn);
··· AngletoFace = FAngle + AngletoTurn;
··· Format.printf("Angle e robot sd face: %d\n",AngletoFace);
····· while (true) {
······· response();
··········· if ((angle >= (AngletoFace - 2)) && (angle <= (AngletoFace + 2)))
··········· {//stop turn;
·············· Format.printf("Stop turn %d\n");
············· return;
··········· }
··········· else
··········· {
············· Format.printf("Angle e robot sd face: %d\n",AngletoFace);
············· turnClockwise();
··········· }
····· }
}
========================================================

i wanted the robot to stop turning once it has met the condition, in the case·shown in·attachment:· 119<angle<123. But it seems to keep going into my ELSE statement.

Peter Verkaik

I don't think you should start and stop pwm like that.
Normally you start the pwm with the centerposition and then
use update when necessary.

Your angle parameter is in 0.01 units, while your AngleToTurn and AngleToFace are
in 1 units. I think you must use

··········· if (((angle/100) >= (AngletoFace - 2)) && ((angle/100) <= (AngletoFace + 2)))


regards peter

StereoPonyz

Oh Yes... Stupid me... i remembered to divide the angle by 100 in the earlier codes. This one i had forgotten. Thanks.

For the servo rotation, do you mean not to· use stop command? den start off the servo as stationary first. But as for the duration of the rotation i can just input a small value for it to just move a little, afterwhich it will stop by itself?

Post Edited (StereoPonyz) : 1/14/2010 9:14:15 AM GMT

Peter Verkaik

Get tha javelinbot manual
http://forums.parallax.com/showthread.php?p=829542

which provides code and explanations about servo control.

I think using update(173,2304) also stops a servo, and that way
there is no time lost for starting and stopping.
(But the way you use it is also used initially in the above manual, chapter 2)

regards peter

StereoPonyz

Yes, i have already looked through the manual some time back... i understand that update(173, 2304) is to update the servo with the center pulse, and values smaller or bigger than it means left turn or right turn respectively. However, due to lack of codes sample, which is why i followed the start/stop method that's shown in manual.

What i don't understand is on how to control the duration of the rotation. I am searching on CPU.delay(number), in which the number represents what time duration? There is explanation of it that i can find.

So, from one of the example 2.3, the explanation states "cause the LED to flash on/off at 5 Hz" and it used CPU.delay(1000), so to obtain 5hz, 1000 must be /5000, which i am unsure if it is really the case.

But, thanks anyway, i am trying the duration myself using the different CPU.delay number

Post Edited (StereoPonyz) : 1/14/2010 5:14:19 PM GMT

Peter Verkaik

CPU.delay(countvalue) waits 11 clockcycles for every count

11*8.68 usec = 95.48 usec

To delay a specific time (in usec)

countvalue = time/95.48

For example, to delay 10 msec = 10000 usec

countvalue = 10000/95.48 = 105· --> CPU.delay(105) delays for 10 msec

regards peter

StereoPonyz

Oh thank you Peter!

Peter may i find out why is there Error in Trigo calculation when xpos, ypos = (-,+) and xpos, ypos = (-,-)?
However, xpos, ypos = (-,+) and xpos, ypos = (+,+) will have Correct answers.
I had attached the javelin terminal.

=======================================================================
static void PlusPlus() {
int AngletoTurn, AngletoFace;
int TestAngletoTurn;
AngletoTurn = (Trig.atan2(xpos,ypos))/10;
Format.printf("Amount of angle to turn: %d\n",AngletoTurn);
AngletoFace = FAngle + AngletoTurn; //Towrds +180
Format.printf("Angle e robot sd face: %d\n",AngletoFace);

while (true) {
response();
if ((angle/100) >= (AngletoFace + 2))
{//stop turn;
Format.printf("Stop turn %d\n");
return;

}

else
{
Format.printf("Angle e robot sd face: %d\n",AngletoFace);
turnClockwise();
}
}
}

static void PlusMinus() {
int AngletoTurn, AngletoFace;
int TestAngletoTurn;
AngletoTurn = (Trig.atan2(xpos,ypos))/10;
Format.printf("Amount of angle to turn: %d\n",AngletoTurn);
AngletoFace = -1*FAngle + AngletoTurn; //Towards -180
Format.printf("Angle e robot sd face: %d\n",AngletoFace);

while (true) {
response();
if ((angle/100) <= (AngletoFace - 2))
{//stop turn;
Format.printf("Stop turn %d\n");
return;

}

else
{
Format.printf("Angle e robot sd face: %d\n",AngletoFace);
turnAntiClockwise();
}
}

}

static void MinusPlus() {
int AngletoTurn, AngletoFace;
int TestAngletoTurn;
AngletoTurn = (Trig.atan2(xpos,ypos))/10;
Format.printf("Amount of angle to turn: %d\n",AngletoTurn);
AngletoFace = FAngle + AngletoTurn; //towards 0
Format.printf("Angle e robot sd face: %d\n",AngletoFace);

while (true) {
response();
if ((angle/100) <= (AngletoFace))
{//stop turn;
Format.printf("Stop turn %d\n");
return;

}

else
{
Format.printf("Angle e robot sd face: %d\n",AngletoFace);
turnAntiClockwise();
}
}
}



static void MinusMinus() {
int AngletoTurn, AngletoFace;
int TestAngletoTurn;
AngletoTurn = (Trig.atan2(xpos,ypos))/10;
Format.printf("Amount of angle to turn: %d\n",AngletoTurn); //+ve
AngletoFace = -(FAngle - AngletoTurn); //Towards -ve0
Format.printf("Angle e robot sd face: %d\n",AngletoFace);

while (true) {
response();
if ((angle/100) <= (AngletoFace - 2))
{//stop turn;
Format.printf("Stop turn %d\n");
return;

}

else
{
Format.printf("Angle e robot sd face: %d\n",AngletoFace);
turnClockwise();
}
}
}
==========================================================================

Peter Verkaik

The result -109 which should be 19:
add 90 to -109 and you get -19 -> sign error and quadrant error

The result 118 which should be 61:
subtract 180·and you get -62 -> sign error and quadrant error

So there appears to be an error when xpos (or ypos) < 0

I noticed you use atan2(xpos,ypos) but note
that atan2(int y, int x) is defined with x and y swapped

Attached is a test program for the Trig.java class.

I don't have a javelin at hand, but you can program it in your javelin
and check the calculated values for a great number of angles.

regards peter

StereoPonyz

Hi, Peter, may i know why i am not able to use 2 decimal places mtd for calculating x and y?
If i divide by 10000 i will get correct values but it is in 4 decimal places.
--> Format.printf("%3d.",UnsignedIntMath.abs(i)/10000);
Format.printf("%02d",UnsignedIntMath.abs(i)%10000);


Why is the results seems to be in 4 decimal places even though i never use %d. and %04d for printf ?


I would like to do some values comparison in 2 decimal places.
The abv mtd is just printing the integer and integer with decimal append at the back, can i link them up as one floating number?

Post Edited (StereoPonyz) : 1/16/2010 5:43:50 PM GMT

Peter Verkaik

Format.printf("%3d",somevalue) will use 3 chars if somevalue >= -99 and somevalue < 1000
Other values result in using more chars (so you get the right value but format is lost)

Say you have a result with 4 decimals, like cos(x), but you only want 2 decimals.
Simply divide cos(x) by 100, eg.
int cosx = cos(x)/100; //loose 2 last digits
Rounded value:
int cosx = (cos(x)+50)/100; //loose 2 last digits

Format.printf("%d.",cosx/100); //print integer part and decimal point
Format.printf("%02d",UnsignedIntMath.abs(cosx)%100); //print 2 decimals

regards peter

StereoPonyz

Hi Peter, may i know why there is no value printed out by f and max?

static void findQuad() {
System.out.println("\nfinding Quadrant");

while (true) {
response();
X = xpos/100;
Y = ypos/100;

mulX = X * X;
mulY = Y * Y;
mulr = r * r;

f = mulX + mulY - mulr;
Max = Math.max(zero, f);

Format.printf("f Value:\n",f);
Format.printf("Max Value\n",Max);


}
}

public static void main(){
findQuad();

}

Peter Verkaik

You need to set a format specifier (%d, %i, %s, %c, %x, %o, %b)

Format.printf("f Value: %d\n",f);
Format.printf("Max Value %d\n",Max);

regards peter

StereoPonyz

Hi Peter, the maths calculation seems to give wrong answer... However, using the same maths in one of subroutine works fine..

The int values for x and y are 0.

===================================================================================
X = xpos/100 - x;
Y = ypos/100 - y;

mulX = X * X;
mulY = Y * Y;
mulr = r * r;

int f = mulX + mulY - mulr;
int Max = Math.max(0, f);

Format.printf("f Value: %d\n",f);
Format.printf("Max Value %d\n",Max);

int Fx = Kp*Max*2*(xpos/100 - x);
int Fy = Kp*Max*2*(ypos/100 - x);

if ((Fx < 0) && (Fy > 0))
{
MinusPlus();
}

StereoPonyz

Peter PLZ help~~~~~~ i'm still not able to get the calculations.....

Peter Verkaik

I don't understand what you want to do.

int f = mulX + mulY - mulr;
int Max = Math.max(0, f);

Format.printf("f Value: %d\n",f);
Format.printf("Max Value %d\n",Max);

always leads to f == Max for f >= 0

regards peter