Sensor-Demo/DemoApplication.cpp at master · OlinRoboticsAndBioinspiration/Sensor-Demo · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
#include "DemoApplication.h"
#include <fstream>
#include <iostream>
#include <string>

DemoApplication::DemoApplication(void)
{
  rot_x = 0;
  rot_y = 0;
  rot_z = 0;
  resetOrientation();
}

DemoApplication::~DemoApplication(void)
{
}

bool DemoApplication::keyPressed( const OIS::KeyEvent &arg )
{
  if (arg.key == OIS::KC_SPACE)
  {
    resetOrientation();
  }

  int motorOld = motorSpeed;
  if (arg.key == OIS::KC_0)
  {
    motorSpeed = 0;
  }
  if (arg.key == OIS::KC_1 ) {
    motorSpeed = 10;
  }
  if (arg.key == OIS::KC_2 ) {
    motorSpeed = 15;
  }
  if (arg.key == OIS::KC_3 ) {
    motorSpeed = 20;
  }
  if (arg.key == OIS::KC_4 ) {
    motorSpeed = 25;
  }
  if (arg.key == OIS::KC_5 ) {
    motorSpeed = 30;
  }
  if (motorSpeed != motorOld) {
    std::ofstream myfile;
    myfile.open ("out.txt");
    myfile << motorSpeed;
    printf("Motorspeed changed, %d \n", motorSpeed);
    myfile.close();
  }
  return BaseApplication::keyPressed(arg);
}

bool DemoApplication::keyReleased( const OIS::KeyEvent &arg )
{
  oldIssued = false;
  return BaseApplication::keyReleased(arg);
}

void DemoApplication::createScene(void)
{
   // Set the scene's ambient light
    mSceneMgr->setAmbientLight(Ogre::ColourValue(0.5f, 0.5f, 0.5f));

    // Create an Entity
    Ogre::Entity* bodyEntity = mSceneMgr->createEntity("Head", "robot.mesh");


    // Create a SceneNode and attach the Entity to it
    bodyNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("HeadNode");
    bodyNode->attachObject(bodyEntity);

    // Create a Light and set its position
    Ogre::Light* light = mSceneMgr->createLight("MainLight");
    light->setPosition(20.0f, 80.0f, 50.0f);
    // create your scene here :)
}

bool DemoApplication::frameRenderingQueued(const Ogre::FrameEvent& evt)
{
  std::ifstream ifs("sensors.txt");
  std::string contents;
  contents.assign(std::istreambuf_iterator<char>(ifs),
                       std::istreambuf_iterator<char>()  );
  if (contents.size() >= 20) {
    bodyNode->resetOrientation();
    float x, y, z;
    sscanf(contents.c_str(), "%f, %f, %f", &x, &y, &z);
    Ogre::Radian xr = Ogre::Radian(x - rot_x_orig);
    Ogre::Radian yr = Ogre::Radian(y - rot_y_orig);
    Ogre::Radian zr = Ogre::Radian(z - rot_z_orig);

    rot_x = x;
    rot_y = y;
    rot_z = z;

    bodyNode->yaw(xr/2);
    bodyNode->pitch(yr/2); //top down rotaiton
    bodyNode->roll(zr/2);
  }
  return BaseApplication::frameRenderingQueued(evt);
}

void DemoApplication::resetOrientation()
{
  rot_x_orig = rot_x;
  rot_y_orig = rot_y;
  rot_z_orig = rot_z;
}

#if OGRE_PLATFORM == OGRE_PLATFORM_WIN32
#define WIN32_LEAN_AND_MEAN
#include "windows.h"
#endif

#ifdef __cplusplus
extern "C" {
#endif

#if OGRE_PLATFORM == OGRE_PLATFORM_WIN32
    INT WINAPI WinMain( HINSTANCE hInst, HINSTANCE, LPSTR strCmdLine, INT )
#else
    int main(int argc, char *argv[])
#endif
    {
        // Create application object
        DemoApplication app;

        try {
            app.go();
        } catch( Ogre::Exception& e ) {
#if OGRE_PLATFORM == OGRE_PLATFORM_WIN32
            MessageBox( NULL, e.getFullDescription().c_str(), "An exception has occured!", MB_OK | MB_ICONERROR | MB_TASKMODAL);
#else
            std::cerr << "An exception has occured: " <<
                e.getFullDescription().c_str() << std::endl;
#endif
        }

        return 0;
    }

#ifdef __cplusplus
}
#endif