pair<double, double> solve(const int* input) {
vector<point> points;
for (int i = 0; i < 18; i+=3) {
points.push_back(makePoint(input[i+0], input[i+1], input[i+2]));
}
points.push_back(calculateTwoTetrahedraCenter(points[0],points[1],points[2],points[3],points[4]));
double minHeight = 100000.0;
double maxHeight = -1.0;
for (int i = 0; i < ROTATION_SIZE; i++) {
vector<point> rotated;
for (int j = 0; j < POINT_SIZE; j++) {
rotated.push_back(points[ROTATION[i][j]]);
}
if (!normalizePosition(rotated)) {
continue;
}
if (isStable(rotated)) {
minHeight = min(minHeight, rotated[5][2]);
maxHeight = max(maxHeight, rotated[5][2]);
}
}
return make_pair(minHeight, maxHeight);
}
void DragableCardFrame::mouseMoveEvent(QMouseEvent *event) {
if (!pressed)
return;
raise();
QPoint moveToPosition(event->pos() - dragPoint + pos());
this->move(normalizePosition(moveToPosition));
}
LinearActuator::LinearActuator(Sabertooth* driver,
uint8_t motor, uint8_t pos_pin) {
_pos_pin = pos_pin;
_next_pos_index = 0;
_driver = driver;
_motor = motor;
// Set the initial position
int position = analogRead(_pos_pin);
for (int i = 0; i < LA_HISTORY_LEN; i++) {
_pos_history[i] = position;
}
_position = position;
normalizePosition();
// Set the intial target for where we currently are
_target_position = _position;
// setup the default PID parameters.
//_p_gain = 1270.0;
//_i_gain = 0.0;
//_d_gain = 0.0;
_p_gain = 571.5;
_previous_error = 30.5;
_error_integral = 7937.5;
_last_update = millis();
}
/**
* @brief Rotates the camera view direction around Z axis.
* @param new_mouse_pos New mouse position
*/
void rotateZ ( Eigen::Vector2f new_mouse_pos ) override
{
Eigen::Vector2f new_position = normalizePosition(new_mouse_pos);
Eigen::Vector2f dir2d = new_position - start_mouse_pos;
start_mouse_pos = new_position;
rotation_Z_axis = dir2d[1]*M_PI;
if (rotation_Z_axis > 2*M_PI)
rotation_Z_axis -= 2*M_PI;
if (rotation_Z_axis < 0)
rotation_X_axis += 2*M_PI;
}
/**
* @brief Computes and applies the translation transformations to the trackball given new position.
* @param pos New mouse position in normalized trackball system
*/
void translateCamera (const Eigen::Vector2f& pos)
{
Eigen::Vector2f normalized_pos = normalizePosition(pos);
if (!translating)
{
translating = true;
initialTranslationPosition = normalized_pos;
}
else if (pos != initialPosition.head(2))
{
finalTranslationPosition = normalized_pos;
computeTranslationVector();
updateViewMatrix();
initialTranslationPosition = finalTranslationPosition;
}
}
/**
* @brief Computes and applies the rotation transformations to the trackball given new position.
* @param pos New mouse position in normalized trackball system
*/
void rotateCamera (const Eigen::Vector2f& pos)
{
Eigen::Vector3f normalized_pos = computeSpherePosition(normalizePosition(pos));
if (!rotating)
{
rotating = true;
initialPosition = normalized_pos;
}
else if ( pos != initialPosition.head(2))
{
finalPosition = normalized_pos;
computeRotationAngle();
updateViewMatrix();
initialPosition = finalPosition;
}
}
void LinearActuator::update() {
// Write the current position in the circular buffer
_pos_history[_next_pos_index] = analogRead(_pos_pin);
_next_pos_index = (_next_pos_index + 1) % LA_HISTORY_LEN;
// Average the circular buffer for the actual position.
uint32_t total = 0;
for (uint8_t i = 0; i < LA_HISTORY_LEN; i++) {
total += _pos_history[i];
}
_position = total / LA_HISTORY_LEN;
normalizePosition();
// Now that we have a position, execute the PID loop.
// We only run the PID loop every 10ms at the fastest.
uint32_t delta_t = millis() - _last_update;
if (delta_t > 100) {
double error = ((double) _target_position - (double) _position) / LA_RAW_POS_MAX;
Serial.print("error: ");
Serial.println(error);
_error_integral = _error_integral + (error * delta_t);
double derivative = (error - _previous_error) / delta_t;
double output = _p_gain * error +
_i_gain * _error_integral +
_d_gain * derivative;
Serial.print("output: ");
Serial.println(output);
output = constrain(output, -127.0, 127.0);
_driver->motor(_motor, (int8_t) output);
_previous_error = error;
_previous_output = output;
_last_update = millis();
Serial.print("Pos: ");
Serial.print(_position);
Serial.print(" Target Pos: ");
Serial.print(_target_position);
Serial.print(" Speed: ");
Serial.print(output);
Serial.println("");
}
}
void Ball::update() {
position.x += xChange * SPEED;
position.y += yChange * SPEED;
normalizePosition(0, PongGame::WIDTH, 0, PongGame::HEIGHT);
}
