void CLocomotion::updateCoord()
{
switch (m_etat.dir){
case FORWARD :
{
// Serial.print("m_mPulses : ");
// Serial.println(m_mPulses);
unsigned int distance = toDistance(m_mPulses);
// Serial.print("distance : ");
// Serial.println(distance);
m_etat.x += distance*cos(getCurrentTheta()*PI/180);
// Serial.print("x : ");
// Serial.println(m_etat.x);
m_etat.y += distance*sin(getCurrentTheta()*PI/180);
// Serial.print("y : ");
// Serial.println(m_etat.y);
// Serial.print("m_dPulses : ");
// Serial.println(m_dPulses);
m_etat.theta +=toAngle(m_dPulses);
// Serial.println(m_etat.theta);
}
break;
case BACKWARD :
{
unsigned int distance = toDistance(m_mPulses);
m_etat.x -= distance*cos(getCurrentTheta()*PI/180);
m_etat.y -= distance*sin(getCurrentTheta()*PI/180);
m_etat.theta -=toAngle(m_dPulses);
}
break;
case RIGHT :
m_etat.theta += toAngle(m_mPulses);
Serial.println(m_etat.theta);
while(m_etat.theta>=360) m_etat.theta-= 360;
while(m_etat.theta<0) m_etat.theta+= 360;
// Serial.println(m_etat.theta);
break;
case LEFT :
m_etat.theta -= toAngle(m_mPulses);
while(m_etat.theta>=360) m_etat.theta-= 360;
while(m_etat.theta<0) m_etat.theta+= 360;
break;
}
}
void DebugDraw::draw(const Transform &tform, const Rigidbody &rbody, const Matrix4 &proj)
{
Transform t;
t = tform;
t.setAngle(toAngle(rbody.velocity));
t.setScale(Vector2{ mag(rbody.velocity) * 100, 1 });
shader.Bind();
mesh->DebugDraw(proj * t.getDrawMatrix());
t = tform;
t.setAngle(toAngle(rbody.acceleration));
t.setScale(Vector2{ mag(rbody.acceleration) * 100, 1 });
mesh->DebugDraw(proj * t.getDrawMatrix());
}
QwtArray<QwtDoubleInterval> QwtCircleClipper::clipCircle(
const QwtDoublePoint &pos, double radius) const
{
QList<QwtDoublePoint> points;
for ( int edge = 0; edge < NEdges; edge++ )
points += cuttingPoints((Edge)edge, pos, radius);
QwtArray<QwtDoubleInterval> intv;
if ( points.size() <= 0 )
{
QwtDoubleRect cRect(0, 0, 2 * radius, 2* radius);
cRect.moveCenter(pos);
if ( contains(cRect) )
intv += QwtDoubleInterval(0.0, 2 * M_PI);
}
else
{
QList<double> angles;
for ( int i = 0; i < points.size(); i++ )
angles += toAngle(pos, points[i]);
qSort(angles);
const int in = contains(qwtPolar2Pos(pos, radius,
angles[0] + (angles[1] - angles[0]) / 2));
if ( in )
{
for ( int i = 0; i < angles.size() - 1; i += 2)
intv += QwtDoubleInterval(angles[i], angles[i+1]);
}
else
{
for ( int i = 1; i < angles.size() - 1; i += 2)
intv += QwtDoubleInterval(angles[i], angles[i+1]);
intv += QwtDoubleInterval(angles.last(), angles.first());
}
}
return intv;
}
// main loop program for the DAQ
int runArm(void) {
double base_voltage;
double elbow_voltage;
double base_angle;
double base_angle_rad;
double elbow_relative_angle;
double elbow_abs_angle;
double elbow_abs_angle_rad;
double x, y;
double min_x, max_x, min_y, max_y;
int mode;
int calibration_flag = 1;
while (continueSuperLoop()) {
// sleep
//Sleep(300);
// mode switching
mode = digitalRead(SWITCH_0);
// read sensor data
base_voltage = analogRead(BASE);
elbow_voltage = analogRead(ELBOW);
// filter out negative voltages
if (base_voltage <= 0 OR elbow_voltage <= 0) {
if (DEBUG) printf("Error: arm position not possible in real life\n");
}
else {
// convert voltage to angle
base_angle = toAngle(base_voltage, BASE);
elbow_relative_angle = toAngle(elbow_voltage, ELBOW);
// convert relative elbow angle to abs angle
elbow_abs_angle = elbow_relative_angle + base_angle - 180;
// converts angles to radians
base_angle_rad = radian(base_angle);
elbow_abs_angle_rad = radian(elbow_abs_angle);
// convert angles to coordinates
x = BASE_LENGTH * cos(base_angle_rad) + ELBOW_LENGTH * cos(elbow_abs_angle_rad);
y = BASE_LENGTH * sin(base_angle_rad) + ELBOW_LENGTH * sin(elbow_abs_angle_rad);
// check if the calibration mode is on or not
if (NOT mode) {
// finds maximum and minimum of x and y
if (calibration_flag) {
min_x = x;
min_y = y;
max_x = x;
max_y = y;
calibration_flag = 0;
}
else {
// set bounds for x
if (x < min_x) {
min_x = x;
}
else if (x > max_x) {
max_x = x;
}
// set bounds for y
if (y < min_y) {
min_y = y;
}
else if (y > max_y) {
max_y = y;
}
}
}
else {
// calibration mode is off - normalize coordinates
x = (x - min_x) / (max_x - min_x);
y = (y - min_y) / (max_y - min_y);
}
// print sensor data to screen
if (DEBUG) {
printf("\nVb=%5.3f Ve=%5.3f\n"
"Ab=%5.2f Are=%5.2f Ae=%5.2f\n"
"x=%5.3f y=%5.3f\n",
base_voltage, elbow_voltage, base_angle, elbow_relative_angle, elbow_abs_angle, x, y);
if (NOT mode) {
printf("minx=%5.3f miny=%5.3f\nmaxx=%5.3f maxy=%5.3f\n",
min_x, min_y, max_x, max_y);
}
}
}
}
}
