How to output Motor Positions of Pan/Tilt motors as Int values?

Added by Michael Stetu 4 months ago

Hello,
Im trying to output the pan and tilt motor positions from the pan/tilt servo motor as int values. I see they are declared as int32_t, I have attempted to use source 1 listed below to output it as an int value and I'm unable to get it to compile.

I was wondering what the best way to output these values in a useable format would be?

(To do this we are using an Arduino Uno)
(Our usecase eventually will be attaching this to the bottom of a Drone to identify a landing pad using color codes. We just need to get the motor positions relatively so we can adjust the drone position in midair before we initiate the landing procedure.)

Any and all help is appreciated.
-Mstetu

Source 1:
https://stackoverflow.com/questions/9225567/how-to-print-a-int64-t-type-in-c


Replies (4)

RE: How to output Motor Positions of Pan/Tilt motors as Int values? - Added by Jesse French 4 months ago

Hey Michael,

could you post your code so we can see what's going on?

Thanks!
Jesse

RE: How to output Motor Positions of Pan/Tilt motors as Int values? - Added by Michael Stetu 4 months ago

Im currently using the example Pan/Tilt code, the only things i currently have added are just a 10 second delay once it is centered and I have added psudocode that currently isn't working for outputting the Pan angle.

//#include <inttypes.h>
#include <SPI.h>
#include <Pixy.h>

Pixy pixy;

#define X_CENTER ((PIXY_MAX_X-PIXY_MIN_X)/2)
#define Y_CENTER ((PIXY_MAX_Y-PIXY_MIN_Y)/2)

class ServoLoop {
public:
ServoLoop(int32_t pgain, int32_t dgain);

void update(int32_t error);
int32_t m_pos;
int32_t m_prevError;
int32_t m_pgain;
int32_t m_dgain;
};

ServoLoop panLoop(300, 500);
ServoLoop tiltLoop(500, 700);

ServoLoop::ServoLoop(int32_t pgain, int32_t dgain) {
m_pos = PIXY_RCS_CENTER_POS;
m_pgain = pgain;
m_dgain = dgain;
m_prevError = 0x80000000L;
}

void ServoLoop::update(int32_t error) {
long int vel;
char buf32;
if (m_prevError!=0x80000000) {
vel = (error*m_pgain + (error - m_prevError)*m_dgain)>>10;
//sprintf(buf, "%ld\n", vel);
//Serial.print(buf);
m_pos += vel;
if (m_pos>PIXY_RCS_MAX_POS)
m_pos = PIXY_RCS_MAX_POS;
else if (m_pos<PIXY_RCS_MIN_POS)
m_pos = PIXY_RCS_MIN_POS;
}
m_prevError = error;
}

void setup() {
Serial.begin(9600);
Serial.print("Starting...\n");

pixy.init();
}

void loop() {
static int i = 0;
int j;
int g;
float panpos;
uint16_t blocks;
char buf32;
int32_t panError, tiltError;

blocks = pixy.getBlocks();
if (blocks)
{
panError = X_CENTER-pixy.blocks[0].x;
tiltError = pixy.blocks[0].y-Y_CENTER;
panLoop.update(panError);
tiltLoop.update(tiltError);
pixy.setServos(panLoop.m_pos, tiltLoop.m_pos);
i++;
// do this (print) every 50 frames because printing every
// frame would bog down the Arduino
if (i%50==0) {
sprintf(buf, "Detected %d:\n", blocks);
Serial.print(buf);
for (j=0; j&lt;blocks; j++) {
sprintf(buf, " block %d: ", j);
Serial.print(buf);
pixy.blocks[j].print();
for(g=0;g&lt;10;g++)
{
delay(1000);
Serial.print("Locked on: ");
Serial.print(g);
Serial.print("\n PanPos: ");
printf("%d" PRId32 "\n", panError);
Serial.print("\n");
}
}
}
}
}

RE: How to output Motor Positions of Pan/Tilt motors as Int values? - Added by Michael Stetu 4 months ago

Trying to get Servo Angles for Pan/Tilt, and I was trying to typecast these to INT, so I could determine angles.

RE: How to output Motor Positions of Pan/Tilt motors as Int values? - Added by Edward Getz 4 months ago

Hi Michael,
What seems to be the problem-- are you not getting output, or does the output appear to be incorrect?

Note, panLoop.m_pos and tiltLoop.m_pos are going to be better representations of the angles, because these are the numbers that are sent to pixy.setServos(). Note also that RC servos are not readable, unfortunately. You can only command them and they will move to that position after some time, usually short.

Hope this helps :)

Edward

(1-4/4)