Setting Initial Values

system · March 25, 2011, 4:00pm

Hey, I'm working on an AHRS project where a Razor 9DOF is used to sense changes in direction of an AUV.

My problem is that when using the razor to output euler angles, such as the code on the sparkfun website does, the yaw is referenced so that it's zero position is north. Our current code uses the euler angles to actuate itself around the zero positions, which is fine for pitch and roll, but in most cases we want the yaw to actuate around its initial starting value.

My question is does anyone know of a way of reading the initial starting value of yaw, then setting that to the reference value which the rotation in yaw is calculated around.

 #include <NewSoftSerial.h>

NewSoftSerial myserial = NewSoftSerial(17, 16);

// define motor driver pin numbers:
const int lefthorzA = 22;
const int lefthorzB = 23;
const int righthorzA = 24;
const int righthorzB = 25;
const int leftvertA = 26;
const int leftvertB = 27;
const int rightvertA = 28;
const int rightvertB = 29;
const int frontvertA = 30;
const int frontvertB = 31;
const int rearvertA = 32;
const int rearvertB = 33;
// define switch position pin numbers:
const int switchposA = 51;
const int switchposB = 52;
const int switchposC = 53;

delay (5000);

void setup() {
	Serial.begin(9600);
	Serial1.begin(9600);
	Myserial.begin(9600);

// set the digital pins as inputs or outputs:
pinMode(lefthorzA, OUTPUT);
pinMode(lefthorzB, OUTPUT);
pinMode(righthorzA, OUTPUT);
pinMode(righthorzB, OUTPUT);
pinMode(leftvertA, OUTPUT);
pinMode(leftvertB, OUTPUT);
pinMode(rightvertA, OUTPUT);
pinMode(rightvertB, OUTPUT);
pinMode(frontvertA, OUTPUT);
pinMode(frontvertB, OUTPUT);
pinMode(rearvertA, OUTPUT);
pinMode(rearvertB, OUTPUT);
pinMode(switchposA, INPUT);
pinMode(switchposB, INPUT);
pinMode(switchposC, INPUT);
}

bool ReadingString = false;   // Are we building a string? 
char Buffer[17];            // Build up the string from the Razor here
byte BufferIndex = 0;         // Next free position in the buffer
int roll = 0;              // Value from the Razor
int pitch = 0;              // Value from the Razor
int yaw = 0;            // Value from the Razor
int valueA = 0;
int valueA = 0;
int valueA = 0;

void loop() {

		// read the output of the switch:
valueA = digitalRead(switchposA);	 // read the input from switch position pin A 
valueB = digitalRead(switchposB);	// read the input from switch position pin B
valueC = digitalRead(switchposC); 	// read the input from switch position pin C

if (valueA = HIGH) {
delay(180000);
unsigned long time;
time = millis();
if (time<120000) {

int c;

if(Serial1.available())	// Something there?
{
c=Serial1.read();	// Get a character
if(c=='!')	// Use the ! to indicate the start of a series to read
{
ReadingString=true;	// Now we're reading to the buffer, rather than just
// throwing stuff away
BufferIndex=0;       // Reset to the start of the buffer
}
else	//Not a !. If we're mid string, store it, otherwise, throw it away
{
if(ReadingString)	//Should we store it?
Buffer[BufferIndex++]=c;
if(BufferIndex>14)	//  Have we read an entire Razor string
{ 
ReadingString=false;	// Done reading
Buffer[BufferIndex]=0; 	// Set a terminator for the last atoi
BufferIndex=0;	// Reset to start of buffer for next string
// Read & display the values we got
roll = atoi(&Buffer[4]);
pitch = atoi(&Buffer[8]);
yaw = atoi(&Buffer[12]);
Serial.print("Angles  ");
Serial.print (roll);
Serial.print("  ");
Serial.print (pitch);
Serial.print("  ");
Serial.print (yaw);
Serial.println("  ");
mySerial.print("Angles  ");
mySerial.print (roll);
mySerial.print("  ");
mySerial.print (pitch);
mySerial.print("  ");
mySerial.print (yaw);
mySerial.println("  ");
}
}  
}
}
}
if (180<roll<=355) {
// roll craft left:
digitalWrite (leftvertA, HIGH);
digitalWrite (leftvertB, LOW); 
digitalWrite (rightvertA, HIGH);
digitalWrite (rightvertB, LOW);
}
if (365>=roll<540) {
// roll craft right:
digitalWrite (leftvertA, LOW);
digitalWrite (leftvertB, HIGH); 
digitalWrite (rightvertA, LOW);
digitalWrite (rightvertB, HIGH);
}
if (180<pitch<=355) {
// tilt nose down:
digitalWrite (frontvertA, HIGH);
digitalWrite (frontvertB, LOW);
digitalWrite (rearvertA, HIGH);
digitalWrite (rearvertB, LOW);
}
if (365>=pitch<540) {
// tilt nose up:
digitalWrite (frontvertA, LOW);
digitalWrite (frontvertB, HIGH);
digitalWrite (rearvertA, LOW);
digitalWrite (rearvertB, HIGH);
}
if (180<yaw<=355) {
// rotate craft clockwise:
digitalWrite (lefthorzA, LOW);
digitalWrite (lefthorzB, HIGH);
digitalWrite (righthorzA, LOW);
digitalWrite (righthorzB, HIGH);;
}
if (365>=yaw<540) {
// rotate craft anticlockwise:
digitalWrite (lefthorzA, HIGH);
digitalWrite (lefthorzB, LOW);
digitalWrite (righthorzA, HIGH);
digitalWrite (righthorzB, LOW);
}

else {
	// turn thrusters off:
digitalWrite (lefthorzA, LOW);
digitalWrite (lefthorzB, LOW);
digitalWrite (righthorzA, LOW);
digitalWrite (righthorzB, LOW);
digitalWrite (leftvertA, LOW);
digitalWrite (leftvertB, LOW);
digitalWrite (rightvertA, LOW);
digitalWrite (rightvertB, LOW);
digitalWrite (frontvertA, LOW);
digitalWrite (frontvertB, LOW);
digitalWrite (rearvertA, LOW);
digitalWrite (rearvertB, LOW);
}

}
else if (valueB = HIGH){
delay(180000);
unsigned long time;
time = millis();

int c;

if(Serial1.available())	// Something there?
{
c=Serial1.read();	// Get a character
if(c=='!')	// Use the ! to indicate the start of a series to read
{
ReadingString=true;	// Now we're reading to the buffer, rather than just throwing stuff away
BufferIndex=0;       // Reset to the start of the buffer
}
else	//Not a !. If we're mid string, store it, otherwise, throw it away
{
if(ReadingString)	//Should we store it?
Buffer[BufferIndex++]=c;
if(BufferIndex>14)	//  Have we read an entire Razor string
{
ReadingString=false;	// Done reading
Buffer[BufferIndex]=0; 	// Set a terminator for the last atoi
BufferIndex=0;	// Reset to start of buffer for next string
// Read & display the values we got
roll=atoi(&Buffer[4]);
pitch=atoi(&Buffer[8]);
yaw=atoi(&Buffer[12]);
Serial.print("Angles  ");
Serial.print (roll);
Serial.print("  ");
Serial.print (pitch);
Serial.print("  ");
Serial.print (yaw);
Serial.println("  ");
mySerial.print("Angles  ");
mySerial.print (roll);
mySerial.print("  ");
mySerial.print (pitch);
mySerial.print("  ");
mySerial.print (yaw);
mySerial.println("  ");
}
}  
}

if (time<60000) {
// fire horizontal thrusters forwards for 20s:
digitalWrite (lefthorzA, HIGH);
digitalWrite (lefthorzB, LOW);
digitalWrite (righthorzA, LOW);
digitalWrite (righthorzB, HIGH);
}

else if (60000>=time<120000 ){
// fire horizontal thrusters backwards for 20s:
digitalWrite (lefthorzA, LOW);
digitalWrite (lefthorzB, HIGH);
digitalWrite (righthorzA, HIGH);
digitalWrite (righthorzB, LOW);
}

else {
	// turn thrusters off:
digitalWrite (lefthorzA, LOW);
digitalWrite (lefthorzB, LOW);
digitalWrite (righthorzA, LOW);
digitalWrite (righthorzB, LOW);
}


if (time<120000) {

}
if (180<roll<=355) {
// roll craft left:
digitalWrite (leftvertA, HIGH);
digitalWrite (leftvertB, LOW); 
digitalWrite (rightvertA, HIGH);
digitalWrite (rightvertB, LOW);
}
if (365>=roll<540) {
// roll craft right:
digitalWrite (leftvertA, LOW);
digitalWrite (leftvertB, HIGH); 
digitalWrite (rightvertA, LOW);
digitalWrite (rightvertB, HIGH);
}
if (180<pitch<=355) {
// tilt nose down:
digitalWrite (frontvertA, HIGH);
digitalWrite (frontvertB, LOW);
digitalWrite (rearvertA, HIGH);
digitalWrite (rearvertB, LOW);
}
if (365>=pitch<540) {
// tilt nose up:
digitalWrite (frontvertA, LOW);
digitalWrite (frontvertB, HIGH);
digitalWrite (rearvertA, LOW);
digitalWrite (rearvertB, HIGH);
}

else {
	// turn thrusters off:
digitalWrite (leftvertA, LOW);
digitalWrite (leftvertB, LOW);
digitalWrite (rightvertA, LOW);
digitalWrite (rightvertB, LOW);
digitalWrite (frontvertA, LOW);
digitalWrite (frontvertB, LOW);
digitalWrite (rearvertA, LOW);
digitalWrite (rearvertB, LOW);
}

}
else {
;	// do nothing
}
}

wildbill · March 25, 2011, 4:23pm

Are you looking for a way to tell the Razor what it's reference heading is or are you simply trying to keep your vessel on the same heading it was pointing at on startup? Can't help you with the former, but if it's the latter, can't you read the Razor once in setup and store the initial yaw. Then in loop, compare your current yaw against initial & steer left or right accordingly. BTW, I could swear some of that code looks familiar

system · March 25, 2011, 4:44pm

Haha yeah, it's the same group project as in the Serial.read thread.

We have played around a little with trying to keep the vessel pointing at its initial heading, but we couldn't get it to work.

We added a read code within the set up portion of the code, and attempted to print a yaw value (call yawreference), that we could use within the loop portion of the code to subtract from the varying yaw value. However, once we added those pieces of code no values printed within the monitor.

wildbill · March 25, 2011, 5:00pm

If you used the exact same read code in setup, the issue is likely that setup was finished long before the complete Razor string arrived, so you got nothing. You'll need to adapt it to wait for the whole thing. Alternatively, you could initialize yawreference to a value you'll never see from the razor such as -1 and check it in loop. Once you've read a real yaw value, if yawreference is -1, set it to yaw. It means that you'll be doing a pointless comparison on every subsequent call to loop, but it'll get the job done.

system · March 25, 2011, 5:02pm

if (180<pitch<=355)

Uh-oh.
Legal C, so the compiler doesn't have a problem, but you do.

Indentation. Needs more indentation. Yeah.

system · March 25, 2011, 5:09pm

@wildbill, the code was similar yeah. we started the to read the serials but delayed for 5 seconds to allow the razor to startup, even with that nothing happened, if thats what you mean

gardner · March 25, 2011, 5:53pm

reilly2950:
trying to keep the vessel pointing at its initial heading

Picking up the first heading seen after a re-boot may not be what you want anyway. What would likely be useful would be a pushbutton input that you press when the physical heading is what you want. Then have the code grab that and initialize the target heading, then have some feedback like a flashing LED to indicate that the heading's been adopted.

Or a similar thing through a serial command maybe.

nickgammon · March 25, 2011, 10:47pm

As usual, AWOL is right.

if (180<roll<=355) {
// roll craft left:

Let's see if we can break this down. I think comparisons are evaluated left-to-right as they are the same precedence. So let's assume roll is 5.

First:

180 < 5

That will be false, returning zero.

Now:

0 <=355

That will be true. So the if evaluates to true. However I doubt that is what you meant. Hint: you want to compare "roll" twice and use && to check this and that is true.

Topic		Replies	Views
IMU (Razor-9DOF) Exhibition	1	2184	May 6, 2021
Using values from 9DOF Razor in Arduino Mega Sensors	27	12326	May 6, 2021
Values wrap around Programming	18	3768	May 5, 2021
Reading SF9DOF IMU Output using Multiple Serial Input & SoftwareSerial Issue ... LEDs and Multiplexing	2	1556	May 6, 2021
converting char data type from 9dof to float General Guidance	6	899	May 5, 2021

Setting Initial Values

Related topics