static void
init(void)
{
static GLfloat pos[4] =
{5.0, 5.0, 10.0, 0.0};
static GLfloat red[4] =
{0.8, 0.1, 0.0, 1.0};
static GLfloat green[4] =
{0.0, 0.8, 0.2, 1.0};
static GLfloat blue[4] =
{0.2, 0.2, 1.0, 1.0};
glLightfv(GL_LIGHT0, GL_POSITION, pos);
glEnable(GL_CULL_FACE);
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
glEnable(GL_DEPTH_TEST);
/* make the gears */
gear1 = glGenLists(1);
glNewList(gear1, GL_COMPILE);
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, red);
gear(1.0, 4.0, 1.0, 20, 0.7);
glEndList();
gear2 = glGenLists(1);
glNewList(gear2, GL_COMPILE);
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, green);
gear(0.5, 2.0, 2.0, 10, 0.7);
glEndList();
gear3 = glGenLists(1);
glNewList(gear3, GL_COMPILE);
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, blue);
gear(1.3, 2.0, 0.5, 10, 0.7);
glEndList();
glEnable(GL_NORMALIZE);
}
void GearScene::resume(GLFWwindow * /*window*/) {
static GLfloat pos[4] = { 5.0, 5.0, 10.0, 0.0 };
static GLfloat red[4] = { 0.8, 0.1, 0.0, 1.0 };
static GLfloat green[4] = { 0.0, 0.8, 0.2, 1.0 };
static GLfloat blue[4] = { 0.2, 0.2, 1.0, 1.0 };
_quit = false;
glLightfv(GL_LIGHT0, GL_POSITION, pos);
glEnable(GL_CULL_FACE);
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
glEnable(GL_DEPTH_TEST);
/* make the gears */
if(_gear1 == -1) {
_gear1 = glGenLists(1);
glNewList(_gear1, GL_COMPILE);
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, red);
gear(1.0, 4.0, 1.0, 20, 0.7);
glEndList();
_gear2 = glGenLists(1);
glNewList(_gear2, GL_COMPILE);
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, green);
gear(0.5, 2.0, 2.0, 10, 0.7);
glEndList();
_gear3 = glGenLists(1);
glNewList(_gear3, GL_COMPILE);
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, blue);
gear(1.3, 2.0, 0.5, 10, 0.7);
glEndList();
glEnable(GL_NORMALIZE);
}
_start = _time = glfwGetTime();
_frame = 0;
}
void CRRC_AirplaneSim_MCopter01::update(TSimInputs* inputs,
double dt,
int multiloop)
{
CRRCMath::Vector3 v_V_local_airmass;
CRRCMath::Vector3 v_V_gust_local = CRRCMath::Vector3();
env->CalculateWind(v_P_CG_Rwy.r[0], v_P_CG_Rwy.r[1], v_P_CG_Rwy.r[2],
v_V_local_airmass.r[0], v_V_local_airmass.r[1], v_V_local_airmass.r[2]);
CRRCMath::Vector3 v_F_aero, v_F_engine, v_F_gear; // Force x/y/z
CRRCMath::Vector3 v_M_aero, v_M_engine, v_M_gear; // l/m/n <-> roll/pitch/yaw
TSimInputs OutputOfLocalControllers;
CRRCMath::Vector3 v_URel;
for (int n=0; n<multiloop; n++)
{
ls_step( dt );
ls_aux(v_V_local_airmass, v_V_gust_local);
// Global controllers first...
env->ControllerCallback(dt, this, inputs, &myInputs);
// ...local ones afterwards. aileron/elevator/rudder is output for rotation about x,y,z.
OutputOfLocalControllers.CopyFrom(&myInputs); // in case there is no controller for something
if (myInputs.throttle > 0.05)
{
for (unsigned int n=0; n<controllers.size(); n++)
controllers[n]->Calc(dt, this, &myInputs, &OutputOfLocalControllers);
v_URel = CRRCMath::Vector3(OutputOfLocalControllers.aileron, OutputOfLocalControllers.elevator, OutputOfLocalControllers.rudder);
}
else
{
v_URel = CRRCMath::Vector3();
for (unsigned int n=0; n<controllers.size(); n++)
controllers[n]->Reset();
}
// Because local controllers only remove key presses from the queue in myInputs and not from inputs,
// it needs to be done manually here:
inputs->ClearKeys();
aero(dt, v_F_aero, v_M_aero);
engine(dt, &OutputOfLocalControllers, v_URel, v_F_engine, v_M_engine);
gear(&myInputs, v_F_gear, v_M_gear);
ls_accel(v_F_aero + v_F_engine + v_F_gear, v_M_aero + v_M_engine + v_M_gear);
}
}
weapon_filter_quick_opt_widget::weapon_filter_quick_opt_widget(filter_p f, QWidget* parent)
: QWidget( parent )
, filter_( *static_cast<weapon_filter*>( f.get() ) )
{
QHBoxLayout* lay = new QHBoxLayout(this);
lay->setContentsMargins(0, 0, 0, 0);
group_ = new QButtonGroup(this);
group_->setExclusive(false);
BOOST_FOREACH(Gear g, forbidden( AllGear ) ) {
QToolButton* tb = new QToolButton( this );
tb->setProperty("gear", g);
tb->setIcon( QIcon( icon(g) ) );
tb->setToolTip(gear(g));
tb->setCheckable(true);
tb->setAutoRaise(true);
group_->addButton(tb, g);
lay->addWidget(tb);
}
std::vector<NS_CORE M> CalcKinCharacteristics::calcMh( const NS_CORE Code code, const NS_CORE InternalGearRatios& intRatios, std::vector<NS_CORE KpdZac> kpdZacStepen )
{
std::vector<NS_CORE M> ret;
GearBoxWithChangerSpecialFrictionProcess gb( code );
gb.createChains();
NS_CORE GearNumber gear( 0 );
do
{
NS_CORE MappedSystem_p systemM = NS_CORE MappedSystem::createMKpd( gb.getChainsForCurrentGear(), intRatios, kpdZacStepen[gear.getValue()], s_inTorque );
NS_CORE Gaus::solve( systemM );
ret.push_back( systemM->getSolution() );
++gear;
} while ( gb.turnOnNextGear() );
return ret;
}
void CRRC_AirplaneSim_MCopter01::ls_step_init()
{
CRRCMath::Vector3 v_F_aero, v_F_engine, v_F_gear; // Force x/y/z
CRRCMath::Vector3 v_M_aero, v_M_engine, v_M_gear; // l/m/n <-> roll/pitch/yaw
TSimInputs ZeroInput = TSimInputs();
EOM01::ls_step_init();
v_V_local = CRRCMath::Vector3();
/* Initialize vehicle model */
ls_aux(CRRCMath::Vector3(), CRRCMath::Vector3());
aero(0, v_F_aero, v_M_aero);
gear(&ZeroInput, v_F_gear, v_M_gear);
/* Calculate initial accelerations */
ls_accel(v_F_aero + v_F_gear, v_M_aero + v_M_gear);
/* Initialize auxiliary variables */
ls_aux(CRRCMath::Vector3(), CRRCMath::Vector3());
}
void
trap_render (int w, int h, int fill_gradient)
{
double *ctrlpts = animpts;
int len = (NUMPTS * 2);
double prevx = ctrlpts[len - 2];
double prevy = ctrlpts[len - 1];
double curx = ctrlpts[0];
double cury = ctrlpts[1];
double midx = (curx + prevx) / 2.0;
double midy = (cury + prevy) / 2.0;
int i;
cairo_set_fill_rule (cr, CAIRO_FILL_RULE_EVEN_ODD);
cairo_set_source_rgba (cr, 1.0, 1.0, 1.0, 1.0);
cairo_set_operator (cr, CAIRO_OPERATOR_SOURCE);
cairo_rectangle (cr, 0, 0, w, h);
cairo_fill (cr);
cairo_set_operator (cr, CAIRO_OPERATOR_OVER);
cairo_set_source_rgba (cr, 0.75, 0.75, 0.75, 1.0);
cairo_set_line_width (cr, 1.0);
cairo_save (cr);
cairo_scale (cr, (double) w / 512.0, (double) h / 512.0);
cairo_save (cr);
cairo_translate (cr, 170.0, 330.0);
cairo_rotate (cr, gear1_rotation);
gear (30.0, 120.0, 20, 20.0);
cairo_set_source_rgb (cr, 0.75, 0.75, 0.75);
cairo_fill_preserve (cr);
cairo_set_source_rgb (cr, 0.25, 0.25, 0.25);
cairo_stroke (cr);
cairo_restore (cr);
cairo_save (cr);
cairo_translate (cr, 369.0, 330.0);
cairo_rotate (cr, gear2_rotation);
gear (15.0, 75.0, 12, 20.0);
cairo_set_source_rgb (cr, 0.75, 0.75, 0.75);
cairo_fill_preserve (cr);
cairo_set_source_rgb (cr, 0.25, 0.25, 0.25);
cairo_stroke (cr);
cairo_restore (cr);
cairo_save (cr);
cairo_translate (cr, 170.0, 116.0);
cairo_rotate (cr, gear3_rotation);
gear (20.0, 90.0, 14, 20.0);
cairo_set_source_rgb (cr, 0.75, 0.75, 0.75);
cairo_fill_preserve (cr);
cairo_set_source_rgb (cr, 0.25, 0.25, 0.25);
cairo_stroke (cr);
cairo_restore (cr);
cairo_restore (cr);
gear1_rotation += 0.01;
gear2_rotation -= (0.01 * (20.0 / 12.0));
gear3_rotation -= (0.01 * (20.0 / 14.0));
stroke_and_fill_step (w, h);
cairo_new_path (cr);
cairo_move_to (cr, midx, midy);
for (i = 2; i <= (NUMPTS * 2); i += 2) {
double x2, x1 = (midx + curx) / 2.0;
double y2, y1 = (midy + cury) / 2.0;
prevx = curx;
prevy = cury;
if (i < (NUMPTS * 2)) {
curx = ctrlpts[i + 0];
cury = ctrlpts[i + 1];
} else {
curx = ctrlpts[0];
cury = ctrlpts[1];
}
midx = (curx + prevx) / 2.0;
midy = (cury + prevy) / 2.0;
x2 = (prevx + midx) / 2.0;
y2 = (prevy + midy) / 2.0;
cairo_curve_to (cr, x1, y1, x2, y2, midx, midy);
}
cairo_close_path (cr);
if (fill_gradient) {
double x1, y1, x2, y2;
cairo_pattern_t *pattern;
cairo_fill_extents (cr, &x1, &y1, &x2, &y2);
pattern = cairo_pattern_create_linear (x1, y1, x2, y2);
cairo_pattern_add_color_stop_rgb (pattern, 0.0, 1.0, 0.0, 0.0);
cairo_pattern_add_color_stop_rgb (pattern, 1.0, 0.0, 0.0, 1.0);
cairo_pattern_set_filter (pattern, CAIRO_FILTER_BILINEAR);
cairo_move_to (cr, 0, 0);
cairo_set_source (cr, pattern);
cairo_pattern_destroy (pattern);
} else {
cairo_set_source_rgba (cr, FILL_R, FILL_G, FILL_B, FILL_OPACITY);
}
cairo_fill_preserve (cr);
cairo_set_source_rgba (cr, STROKE_R, STROKE_G, STROKE_B, STROKE_OPACITY);
cairo_set_line_width (cr, LINEWIDTH);
cairo_stroke (cr);
}
int main(int, char**)
{
// plan for hybrid car in SE(2) with discrete gears
ob::StateSpacePtr SE2(new ob::SE2StateSpace());
ob::StateSpacePtr velocity(new ob::RealVectorStateSpace(1));
// set the range for gears: [-1,3] inclusive
ob::StateSpacePtr gear(new ob::DiscreteStateSpace(-1,3));
ob::StateSpacePtr stateSpace = SE2 + velocity + gear;
// set the bounds for the R^2 part of SE(2)
ob::RealVectorBounds bounds(2);
bounds.setLow(-100);
bounds.setHigh(100);
SE2->as<ob::SE2StateSpace>()->setBounds(bounds);
// set the bounds for the velocity
ob::RealVectorBounds velocityBound(1);
velocityBound.setLow(0);
velocityBound.setHigh(60);
velocity->as<ob::RealVectorStateSpace>()->setBounds(velocityBound);
// create start and goal states
ob::ScopedState<> start(stateSpace);
ob::ScopedState<> goal(stateSpace);
// Both start and goal are states with high velocity with the car in third gear.
// However, to make the turn, the car cannot stay in third gear and will have to
// shift to first gear.
start[0] = start[1] = -90.; // position
start[2] = boost::math::constants::pi<double>()/2; // orientation
start[3] = 40.; // velocity
start->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3; // gear
goal[0] = goal[1] = 90.; // position
goal[2] = 0.; // orientation
goal[3] = 40.; // velocity
goal->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2)->value = 3; // gear
oc::ControlSpacePtr cmanifold(new oc::RealVectorControlSpace(stateSpace, 2));
// set the bounds for the control manifold
ob::RealVectorBounds cbounds(2);
// bounds for steering input
cbounds.setLow(0, -1.);
cbounds.setHigh(0, 1.);
// bounds for brake/gas input
cbounds.setLow(1, -20.);
cbounds.setHigh(1, 20.);
cmanifold->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
oc::SimpleSetup setup(cmanifold);
setup.setStartAndGoalStates(start, goal, 5.);
setup.setStateValidityChecker(boost::bind(
&isStateValid, setup.getSpaceInformation().get(), _1));
setup.setStatePropagator(boost::bind(
&propagate, setup.getSpaceInformation().get(), _1, _2, _3, _4));
setup.getSpaceInformation()->setPropagationStepSize(.1);
setup.getSpaceInformation()->setMinMaxControlDuration(2, 3);
// try to solve the problem
if (setup.solve(30))
{
// print the (approximate) solution path: print states along the path
// and controls required to get from one state to the next
oc::PathControl& path(setup.getSolutionPath());
// print out full state on solution path
// (format: x, y, theta, v, u0, u1, dt)
for(unsigned int i=0; i<path.getStateCount(); ++i)
{
const ob::State* state = path.getState(i);
const ob::SE2StateSpace::StateType *se2 =
state->as<ob::CompoundState>()->as<ob::SE2StateSpace::StateType>(0);
const ob::RealVectorStateSpace::StateType *velocity =
state->as<ob::CompoundState>()->as<ob::RealVectorStateSpace::StateType>(1);
const ob::DiscreteStateSpace::StateType *gear =
state->as<ob::CompoundState>()->as<ob::DiscreteStateSpace::StateType>(2);
std::cout << se2->getX() << ' ' << se2->getY() << ' ' << se2->getYaw()
<< ' ' << velocity->values[0] << ' ' << gear->value << ' ';
if (i==0)
// null controls applied for zero seconds to get to start state
std::cout << "0 0 0";
else
{
// print controls and control duration needed to get from state i-1 to state i
const double* u =
path.getControl(i-1)->as<oc::RealVectorControlSpace::ControlType>()->values;
std::cout << u[0] << ' ' << u[1] << ' ' << path.getControlDuration(i-1);
}
std::cout << std::endl;
}
if (!setup.haveExactSolutionPath())
{
std::cout << "Solution is approximate. Distance to actual goal is " <<
setup.getProblemDefinition()->getSolutionDifference() << std::endl;
}
}
return 0;
}
static void
init(int argc, char *argv[])
{
static GLfloat pos[4] = {5.0, 5.0, 10.0, 0.0};
static GLfloat red[4] = {0.8, 0.1, 0.0, 1.0};
static GLfloat green[4] = {0.0, 0.8, 0.2, 1.0};
static GLfloat blue[4] = {0.2, 0.2, 1.0, 1.0};
GLint i;
glLightfv(GL_LIGHT0, GL_POSITION, pos);
#if 0
glEnable(GL_CULL_FACE);
#endif
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
glEnable(GL_DEPTH_TEST);
/* make the gears */
Gear1 = glGenLists(1);
glNewList(Gear1, GL_COMPILE);
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, red);
gear(1.0, 4.0, 1.0, 20, 0.7);
glEndList();
Gear2 = glGenLists(1);
glNewList(Gear2, GL_COMPILE);
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, green);
gear(0.5, 2.0, 2.0, 10, 0.7);
glEndList();
Gear3 = glGenLists(1);
glNewList(Gear3, GL_COMPILE);
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, blue);
gear(1.3, 2.0, 0.5, 10, 0.7);
glEndList();
cubeList = glGenLists(1);
glNewList(cubeList, GL_COMPILE);
genCube();
glEndList();
glEnable(GL_NORMALIZE);
/* xxx make size dynamic */
TexWidth = 256;
TexHeight = 256;
glBindTexture(GL_TEXTURE_2D, TexObj);
glTexImage2D(GL_TEXTURE_2D, 0, IntFormat, TexWidth, TexHeight, 0,
GL_RGBA, GL_UNSIGNED_BYTE, NULL);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
glGenFramebuffersEXT(1, &GearsFbo);
glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, GearsFbo);
glGenRenderbuffersEXT(1, &GearsDepthRB);
glBindRenderbufferEXT(GL_RENDERBUFFER_EXT, GearsDepthRB);
glRenderbufferStorageEXT(GL_RENDERBUFFER_EXT, GL_DEPTH_COMPONENT, TexWidth, TexHeight);
glFramebufferRenderbufferEXT(GL_FRAMEBUFFER_EXT, GL_DEPTH_ATTACHMENT_EXT, GL_RENDERBUFFER_EXT, GearsDepthRB);
glFramebufferTexture2DEXT(GL_FRAMEBUFFER_EXT, GL_COLOR_ATTACHMENT0_EXT, GL_TEXTURE_2D, TexObj, 0);
GLenum status = glCheckFramebufferStatusEXT(GL_FRAMEBUFFER_EXT);
if(status != GL_FRAMEBUFFER_COMPLETE_EXT) {
printf("!!! FB not complete\n");
exit(1);
}
glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);
for ( i=1; i<argc; i++ ) {
if (strcmp(argv[i], "-info")==0) {
printf("GL_RENDERER = %s\n", (char *) glGetString(GL_RENDERER));
printf("GL_VERSION = %s\n", (char *) glGetString(GL_VERSION));
printf("GL_VENDOR = %s\n", (char *) glGetString(GL_VENDOR));
printf("GL_EXTENSIONS = %s\n", (char *) glGetString(GL_EXTENSIONS));
}
}
}
void
oneFrame(void)
{
#ifdef STEREO
int i;
/* enclose original rendering code in 2-pass loop */
for (i=0; i<2; i++)
{
/* adjust projection matrix for stereo viewpoint */
if (bStereoEnabled) {
glDrawBuffer((i==0) ? GL_BACK_LEFT : GL_BACK_RIGHT);
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadMatrixf((i==0) ? &mProjectionLeft : &mProjectionRight);
glMatrixMode(GL_MODELVIEW);
}
#endif
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glPushMatrix();
glTranslatef(0.0, 0.0, -4.0);
glRotatef(a3, 1.0, 1.0, 1.0);
glRotatef(a4, 0.0, 0.0, -1.0);
glTranslatef(0.14, 0.2, 0.0);
gear(76,
0.4, 2.0, 1.1,
0.0, 0.4, 0.04,
sizeof(gear_profile) / sizeof(Profile), gear_profile);
glPopMatrix();
glPushMatrix();
glTranslatef(0.1, 0.2, -3.8);
glRotatef(a2, -4.0, 2.0, -1.0);
glRotatef(a1, 1.0, -3.0, 1.0);
glTranslatef(0.0, -0.2, 0.0);
gear(36,
0.4, 2.0, 1.1,
0.0, 0.7, 0.2,
sizeof(gear_profile) / sizeof(Profile), gear_profile);
glPopMatrix();
#ifdef STEREO
/* render twice for stereo, or once for mono */
if (bStereoEnabled) {
glFlush();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
}
else
break;
} /* for loop */
#endif
a1 += i1;
if (a1 > 360.0)
a1 -= 360.0;
if (a1 < 0.0)
a1 -= 360.0;
a2 += i2;
if (a2 > 360.0)
a2 -= 360.0;
if (a2 < 0.0)
a2 -= 360.0;
a3 += i3;
if (a3 > 360.0)
a3 -= 360.0;
if (a3 < 0.0)
a3 -= 360.0;
a4 += i4;
if (a4 > 360.0)
a4 -= 360.0;
if (a4 < 0.0)
a4 -= 360.0;
glutSwapBuffers();
}
void drawFrame()
{
glColor3f(1.0f,0.0f,0.0f);
glPushMatrix();
glPushMatrix();
glColor3f(0.0f,1.0f,0.0f);
glPushMatrix();
glTranslatef(0.0f,0.0f,0.06f);
glRotatef(-2*pedalAngle,0.0f,0.0f,1.0f);
gear(0.08f,0.3f,0.03f,30,0.03f);
glPopMatrix();
glColor3f(1.0f,0.0f,0.0f);
glTranslatef(0.0f,0.0f,-0.2f);
ZCylinder(0.08f,0.32f);
glPopMatrix();
glRotatef(RIGHT_ANGLE,0.0f,0.0f,1.0f);
XCylinder(ROD_RADIUS,RIGHT_ROD);
glRotatef(MIDDLE_ANGLE-RIGHT_ANGLE,0.0f,0.0f,1.0f);
XCylinder(ROD_RADIUS,MIDDLE_ROD);
glColor3f(1.0f,1.0f,0.0f);
glTranslatef(MIDDLE_ROD,0.0f,0.0f);
glRotatef(-MIDDLE_ANGLE,0.0f,0.0f,1.0f);
glScalef(0.3f,ROD_RADIUS,0.25f);
drawSeat();
glColor3f(1.0f,0.0f,0.0f);
glPopMatrix();
glPushMatrix();
glRotatef(-180.0f,0.0f,1.0f,0.0f);
XCylinder(ROD_RADIUS,BACK_CONNECTOR);
glPushMatrix();
glTranslatef(0.5f,0.0f,WHEEL_OFFSET);
XCylinder(ROD_RADIUS,RADIUS_WHEEL+TUBE_WIDTH);
glPopMatrix();
glPushMatrix();
glTranslatef(0.5f,0.0f,-WHEEL_OFFSET);
XCylinder(ROD_RADIUS,RADIUS_WHEEL+TUBE_WIDTH);
glPopMatrix();
glPopMatrix();
glPushMatrix();
glTranslatef(-(BACK_CONNECTOR+RADIUS_WHEEL+TUBE_WIDTH),0.0f,0.0f);
glPushMatrix();
glRotatef(-2*pedalAngle,0.0f,0.0f,1.0f);
drawTyre();
glColor3f(0.0f,1.0f,0.0f);
gear(0.03f,0.15f,0.03f,20,0.03f);
glColor3f(1.0f,0.0f,0.0f);
glPopMatrix();
glRotatef(LEFT_ANGLE,0.0f,0.0f,1.0f);
glPushMatrix();
glTranslatef(0.0f,0.0f,-WHEEL_OFFSET);
XCylinder(ROD_RADIUS,WHEEL_LEN);
glPopMatrix();
glPushMatrix();
glTranslatef(0.0f,0.0f,WHEEL_OFFSET);
XCylinder(ROD_RADIUS,WHEEL_LEN);
glPopMatrix();
glTranslatef(WHEEL_LEN,0.0f,0.0f);
XCylinder(ROD_RADIUS,CRANK_ROD);
glTranslatef(CRANK_ROD,0.0f,0.0f);
glRotatef(-LEFT_ANGLE,0.0f,0.0f,1.0f);
XCylinder(ROD_RADIUS,TOP_LEN);
glTranslatef(TOP_LEN,0.0f,0.0f);
glRotatef(-FRONT_INCLINE,0.0f,0.0f,1.0f);
glPushMatrix();
glTranslatef(-0.1f,0.0f,0.0f);
XCylinder(ROD_RADIUS,0.45f);
glPopMatrix();
glPushMatrix();
glRotatef(-steering,1.0f,0.0f,0.0f);
glTranslatef(-0.3f,0.0f,0.0f);
glPushMatrix();
glRotatef(FRONT_INCLINE,0.0f,0.0f,1.0f);
glPushMatrix();
glTranslatef(0.0f,0.0f,-HANDLE_ROD/2);
ZCylinder(ROD_RADIUS,HANDLE_ROD);
glPopMatrix();
glPushMatrix();
glColor3f(1.0f,1.0f,0.0f);
glTranslatef(0.0f,0.0f,-HANDLE_ROD/2);
ZCylinder(0.07f,HANDLE_ROD/4);
glTranslatef(0.0f,0.0f,HANDLE_ROD*3/4);
ZCylinder(0.07f,HANDLE_ROD/4);
glColor3f(1.0f,0.0f,0.0f);
glPopMatrix();
glPopMatrix();
glPushMatrix();
XCylinder(ROD_RADIUS,CRANK_ROD);
glTranslatef(CRANK_ROD,0.0f,0.0f);
glRotatef(CRANK_ANGLE,0.0f,0.0f,1.0f);
glPushMatrix();
glTranslatef(0.0f,0.0f,WHEEL_OFFSET);
XCylinder(ROD_RADIUS,CRANK_RODS);
glPopMatrix();
glPushMatrix();
glTranslatef(0.0f,0.0f,-WHEEL_OFFSET);
XCylinder(ROD_RADIUS,CRANK_RODS);
glPopMatrix();
glTranslatef(CRANK_RODS,0.0f,0.0f);
glRotatef(-2*pedalAngle,0.0f,0.0f,1.0f);
drawTyre();
glPopMatrix();
glPopMatrix();
glPopMatrix();
}
int main(void) {
init();
while (1) {
// if(usart1_has_data()) {
// char c = getchar();
//
// switch (c) {
// case 27:
// case 91:
// continue;
// break;
// case 65:
// gear_up();
// printf("\n");
// break;
// case 66:
// gear_down();
// printf("\n");
// break;
// case '\r':
// printf("\r\n");
// break;
// default:
// break;
// }
// }
if (can_has_data()) {
struct can_message message;
read_message(&message);
switch (message.id) {
case PADDLE_STATUS:
if (message.data[0] == 1) {
gear_up();
}
if (message.data[0] == 0) {
gear_down();
}
break;
case NEUTRAL_ENABLED:
if (message.data[0] == 1) {
printf("NEUTRAL BUTTON ACTIVATED\n");
while(1) {
if (can_has_data()) {
struct can_message message;
read_message(&message);
switch (message.id) {
case PADDLE_STATUS:
if (message.data[0] == 1) {
gear(UP);
_delay_ms(30);
gear(STOP);
}
if (message.data[0] == 0) {
gear(DOWN);
_delay_ms(30);
gear(STOP);
}
break;
case NEUTRAL_ENABLED:
if (message.data[0] == 0) {
printf("NEUTRAL BUTTON DEACTIVATED\n");
goto NO_NEUTRAL;
}
break;
default:
break;
}
}
}
}
NO_NEUTRAL:
break;
default:
break;
}
can_init();
}
}
return 0;
}
