void follow(struct robot *robot)
{
float conversion_factor = (1000.0 / 90.0) * (180 / M_PI);
if (robot->turret.yaw * conversion_factor > 1960)
{
robot->turret.yaw = M_PI / 2;
robot->turret.pitch = M_PI / 2;
}
if (robot->turret.yaw * conversion_factor < 160)
{
robot->turret.yaw = M_PI / 2;
robot->turret.pitch = M_PI / 4;
}
if (robot->turret.pitch * conversion_factor > 1440)
{
robot->turret.yaw = M_PI / 2;
robot->turret.pitch = M_PI / 4;
}
if (robot->turret.pitch *conversion_factor < 140)
{
robot->turret.yaw = M_PI / 2;
robot->turret.pitch = M_PI / 4;
}
float yaw_setting = robot->turret.yaw * conversion_factor + 340;
float pitch_setting = robot->turret.pitch * conversion_factor + 560;
gpioServo(24, yaw_setting);
gpioServo(4, pitch_setting);
}
void Motor::write(int value){
if(value > 135){
gpioServo(pin, angleToPulseWidth(135));
} else if(value < 45){
gpioServo(pin, angleToPulseWidth(45));
} else {
gpioServo(pin, angleToPulseWidth(value));
}
}
void servoTick(void)
{
static int wid1=1500, wid2=1500, wid3=1500;
static int inc1=50, inc2=75, inc3=100;
gpioServo(SERVO1, wid1);
gpioServo(SERVO2, wid2);
gpioServo(SERVO3, wid3);
wid1+=inc1; if ((wid1<1000) || (wid1>2000)) {inc1 = -inc1; wid1+=inc1;}
wid2+=inc2; if ((wid2<1000) || (wid2>2000)) {inc2 = -inc2; wid2+=inc2;}
wid3+=inc3; if ((wid3<1000) || (wid3>2000)) {inc3 = -inc3; wid3+=inc3;}
}
void set_servo_pos(char sevo_nr, unsigned char pos) //position 0-100 0= 500; 100 = 2500
{
int mics;
int current_time;
gpioTime(PI_TIME_ABSOLUTE , ¤t_time, &mics);
servos[sevo_nr].time = current_time;
servos[sevo_nr].pos = pos;
servos[sevo_nr].on = 1;
if (pos == 255) // -1 = turn off the servo
{
gpioServo(servos[sevo_nr].pin, 0);
servos[sevo_nr].on = 0;
}
else
{
gpioServo(servos[sevo_nr].pin, 500 + 20 * pos);
servos[sevo_nr].on = 1;
}
}
int main(void) {
int rc = gpioInitialise();
if (rc < 0) {
printf("gpioInitialise() failed: %d\n", rc);
exit(-1);
}
int pwmPin = 17;
int servoSweepDuration = 500; // Time in ms
printf("pwmPin pin = %d\n", pwmPin);
printf("servoSweepDuration = %d seconds\n", servoSweepDuration);
int direction = 0; // 0 = up, 1 = down
while(1) {
unsigned int startTime = gpioTick();
unsigned int sweepTime = servoSweepDuration * 1000 / 2;
unsigned int delta = gpioTick() - startTime;
while(delta < sweepTime) {
// Position is going to be between 0 and 1 and represents how far along the sweep
// we are.
double position = (double)delta/sweepTime;
unsigned int timeToWriteHighUSecs;
position = -0.4;
if (direction == 0) {
timeToWriteHighUSecs = (position + 1.0) * 1000;
} else {
timeToWriteHighUSecs = (2.0 - position) * 1000;
}
printf("Setting pulse width %d, %d, %d\n", timeToWriteHighUSecs, delta, startTime);
gpioServo(pwmPin, timeToWriteHighUSecs);
gpioDelay(20 * 1000);
delta = gpioTick() - startTime;
} // End of a sweep in one direction
// Switch direction
if (direction == 0) {
direction = 1;
} else {
direction = 0;
}
} // End of while true
return 0;
}
/*
* Class: com_diozero_pigpioj_PigpioGpio
* Method: setServoPulseWidth
* Signature: (II)I
*/
JNIEXPORT jint JNICALL Java_com_diozero_pigpioj_PigpioGpio_setServoPulseWidth
(JNIEnv* env, jclass clz, jint gpio, jint pulseWidth) {
return gpioServo(gpio, pulseWidth);
}
int point_at_local(struct turret *turret, struct vect *point)
{
int can_point = 1;
// point->x = 0;
// point->y = -40;
// point->z = 400;
struct vect normalized_local;
struct vect diff;
difference(&diff, point, &camera_center);
//printf("%f, %f, %f\n", diff.x, diff.y, diff.z);
//printf("%f, %f, %f\n", point->x, point->y, point->z);
float magnitude = norm(&diff);
if (magnitude == 0)
{
normalized_local.x = 0;
normalized_local.y = 0;
normalized_local.z = 1.0;
}
else
{
normalized_local.x = diff.x / magnitude;
normalized_local.y = diff.y / magnitude;
normalized_local.z = diff.z / magnitude;
}
float conversion_factor = (1000.0 / 90.0) * (180 / M_PI);
turret->yaw = (atan2f(normalized_local.y, -normalized_local.x));
float distance = sqrtf(normalized_local.x * normalized_local.x +
normalized_local.y * normalized_local.y);
turret->pitch = -(atan2f(normalized_local.z, distance));
// fprintf(stderr, "%f\n", turret->pitch);
if (turret->yaw * conversion_factor > 1960)
{
can_point =0;
turret->yaw = M_PI / 2;
turret->pitch = M_PI / 4;
}
if (turret->yaw * conversion_factor < 160)
{
can_point = 0;
turret->yaw = M_PI / 2;
turret->pitch = M_PI / 4;
}
if (turret->pitch * conversion_factor > 1440)
{
can_point = 0;
turret->yaw = M_PI / 2;
turret->pitch = M_PI / 4;
}
if (turret->pitch *conversion_factor < 140)
{
can_point = 0;
turret->yaw = M_PI / 2;
turret->pitch = M_PI / 4;
}
float yaw_setting = turret->yaw * conversion_factor + 415;
float pitch_setting = 2125 - (turret->pitch * conversion_factor);
if (yaw_setting > 500 && yaw_setting < 2500)
{
gpioServo(24, yaw_setting);
}
if (pitch_setting > 500 && pitch_setting < 2500)
{
gpioServo(4, pitch_setting);
}
if (can_point)
{
//gpioWrite(14, 1);
}
else
{
//gpioWrite(14, 0);
}
struct quaternion yaw, pitch;
yaw.w = cosf((turret->yaw - M_PI / 2) / 2);
yaw.x = 0;
yaw.y = 0;
yaw.z = sinf((turret->yaw - M_PI / 2) / 2);
pitch.w = cosf(turret->pitch / 2);
pitch.x = -sinf(turret->pitch / 2);
pitch.y = 0;
pitch.z = 0;
quaternion_product(&turret->turret_rotation, &yaw, &pitch);
return can_point;
}
void Motor::writeNoSafe(int value){
gpioServo(pin, angleToPulseWidth(value));
}
void i2cTick(void)
{
static int inited = 0;
static int calibrated = 0;
static int calibrations = 0;
static int accCalibX = 0, accCalibY = 0, accCalibZ = 0;
static int gyroCalibX = 0, gyroCalibY = 0, gyroCalibZ = 0;
static int i2c;
static float X=0.0, Y=0.0, Z=0.0;
static uint32_t lastTick;
uint32_t tick;
int elapsed;
int pulse;
char str[256];
if (inited)
{
tick = gpioTick();
elapsed = tick - lastTick;
lastTick = tick;
readADXL345(i2c);
readITG3200(i2c);
if (calibrated)
{
X = estimateAngle(
rawAcc[ROLL], rawGyr[ROLL] -gyroCalibX, X, elapsed);
Y = estimateAngle(
rawAcc[PITCH], rawGyr[PITCH] - gyroCalibY, Y, elapsed);
Z = estimateAngle(
rawAcc[YAW], rawGyr[YAW] - gyroCalibZ, Z, elapsed);
pulse = 1500 + (Y * 1000 / 90);
if (pulse < 500) pulse = 500;
if (pulse > 2500) pulse = 2500;
gpioServo(SERVO1, pulse);
pulse = 1500 - (X * 500 / 90);
if (pulse < 1000) pulse = 1000;
if (pulse > 2000) pulse = 2000;
gpioServo(SERVO2, pulse);
/* prefer Z but that doesn't change much */
pulse = 1500 - (Y * 500 / 90);
if (pulse < 800) pulse = 800;
if (pulse > 2200) pulse = 2200;
gpioServo(SERVO3, pulse);
sprintf(str, "X=%4.0f Y=%4.0f Z=%4.0f ", X, Y, Z);
putTTYstr(1, 1, str);
}
else
{
accCalibX+=rawAcc[ROLL];
accCalibY+=rawAcc[PITCH];
accCalibZ+=rawAcc[YAW];
gyroCalibX+=rawGyr[ROLL];
gyroCalibY+=rawGyr[PITCH];
gyroCalibZ+=rawGyr[YAW];
if (++calibrations >= CALIBRATIONS)
{
accCalibX /= CALIBRATIONS;
accCalibY /= CALIBRATIONS;
accCalibZ /= CALIBRATIONS;
gyroCalibX /= CALIBRATIONS;
gyroCalibY /= CALIBRATIONS;
gyroCalibZ /= CALIBRATIONS;
calibrated = 1;
}
}
}
else
{
i2c = initI2Cdevices();
gpioServo(SERVO1, 1500);
gpioServo(SERVO2, 1500);
gpioServo(SERVO3, 1500);
inited = 1;
}
}
