nxtOSEK/samples_c++/cpp/HiTechnicSensors/sample.cpp

   1 /* sample.cpp for TOPPERS/ATK(OSEK) */ \r
   2 \r
   3 // ECRobot++ API\r
   4 #include "AccelSensor.h"\r
   5 #include "CompassSensor.h"\r
   6 #include "ColorSensor.h"\r
   7 #include "IrSeeker.h"\r
   8 #include "Lcd.h"\r
   9 #include "Clock.h"\r
  10 using namespace ecrobot;\r
  11 \r
  12 extern "C"\r
  13 {\r
  14 #include "kernel.h"\r
  15 #include "kernel_id.h"\r
  16 #include "ecrobot_interface.h"\r
  17 \r
  18 AccelSensor     accel(PORT_1);\r
  19 CompassSensor compass(PORT_2);\r
  20 ColorSensor     color(PORT_3);\r
  21 IrSeeker           ir(PORT_4);\r
  22 \r
  23 /* nxtOSEK hook to be invoked from an ISR in category 2 */\r
  24 void user_1ms_isr_type2(void)\r
  25 {\r
  26         SleeperMonitor(); // needed for I2C device and Clock classes\r
  27 }\r
  28 \r
  29 TASK(TaskMain)\r
  30 {\r
  31         Clock clock;\r
  32         Lcd lcd;\r
  33 \r
  34         S16 axes[3]; // Accel Sensor data\r
  35         S16 heading; // Compass Sensor data\r
  36         S16 rgb[3];  // Color Sensor raw color data\r
  37         U8  num;     // Color Sensor data\r
  38         S16 dir;     // IR Seeker data\r
  39         \r
  40         // it may need to calibrate compass sensor\r
  41         compass.beginCalibration();\r
  42         bool calCompass = compass.endCalibration();\r
  43 \r
  44         while(1)\r
  45         {\r
  46                 accel.getAccel(axes);\r
  47                 heading = compass.getHeading();\r
  48                 num = color.getColorNumber();\r
  49                 color.getRawColor(rgb);\r
  50                 dir = ir.getDirection();; \r
  51 \r
  52                 lcd.clear();\r
  53                 lcd.putf("sn",   "HiTechnicSensors");\r
  54                 lcd.putf("sddn", "X/Y: ", axes[0],0, axes[1],5); \r
  55                 lcd.putf("sdn",  "Z:   ", axes[2],0);\r
  56                 lcd.putf("sddn", "C/Hd:", calCompass,0, heading,5);\r
  57                 lcd.putf("sddn", "Nm/R:", num,0, rgb[0],5);\r
  58                 lcd.putf("sddn", "G/B: ", rgb[1],0, rgb[2],5);\r
  59                 lcd.putf("sd",   "Dir: ", dir,0);\r
  60                 lcd.disp();\r
  61 \r
  62                 clock.wait(100);\r
  63         }\r
  64 }\r
  65 }\r