nxtOSEK/ecrobot/ecrobot_interface.c

   1 /*****************************************************************************
   2  * FILE: ecrobot_interface.c
   3  *
   4  * COPYRIGHT 2008 Takashi Chikamasa <takashic@cybernet.co.jp>
   5  *
   6  * <About leJOS NXJ>
   7  *  leJOS NXJ is a full firmware replacement of LEGO Mindstorms NXT and
   8  *  designed for Java programming environment for the NXT
   9  *  ( For more detailed information, please see: http://lejos.sourceforge.net/ )
  10  *  In the leJOS NXJ distribution, C source files for NXT platform layer is also
  11  *  included besides with the Java VM. The platform C source code is well
  12  *  structured, comprehensive, and achieved higher performance than the LEGO's
  13  *  one. Therefore, leJOS NXJ (platform) is also the best GCC based C/C++
  14  *  development platform for NXT.
  15  *
  16  *  The contents of this file are subject to the Mozilla Public License
  17  *  Version 1.0 (the "License"); you may not use this file except in
  18  *  compliance with the License. You may obtain a copy of the License at
  19  *  http://www.mozilla.org/MPL/
  20  *
  21  *  Software distributed under the License is distributed on an "AS IS"
  22  *  basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
  23  *  License for the specific language governing rights and limitations
  24  *  under the License.
  25  *
  26  *  The Original Code is TinyVM code, first released March 6, 2000,
  27  *  later released as leJOS on September 23, 2000.
  28  *
  29  *  The Initial Developer of the Original Code is Jose H. Solorzano.
  30  *
  31  *  Contributor(s): see leJOS NXJ ACKNOWLEDGEMENTS .
  32  *
  33  *
  34  * <About TOPPERS OSEK>
  35  *  TOPPERS OSEK is an open source OSEK kernel and developed by TOPPERS project.
  36  *  TOPPERS(Toyohashi OPen Platform for Embedded Real-time Systems) has been managed
  37  *  by a Non Profit Organization founded in Sep. 2003 and has been led by Professor
  38  *  Hiroaki Takada of Nagoya University in Japan.
  39  *
  40  *  TOPPERS OSEK program is covered by the TOPPERS License as published
  41  *  by the TOPPERS PROJECT (http://www.toppers.jp/en/index.html).
  42  *
  43  *****************************************************************************/
  44
  45 #include <stddef.h>
  46 #include <string.h>
  47
  48 #include "ecrobot_base.h"
  49 #include "ecrobot_private.h"
  50 #include "ecrobot_interface.h"
  51
  52 static U8 deviceStatus = DEVICE_NO_INIT;
  53
  54
  55 /*==============================================================================
  56  * NXT Servo Motor API
  57  *=============================================================================*/
  58
  59 /**
  60  * set Servo Motor PWM duty ratio
  61  *
  62  * @param port_id: NXT_PORT_A/NXT_PORT_B/NXT_PORT_C
  63  * @param speed: PWM duty ration (-100 to 100)
  64  */
  65 void ecrobot_set_motor_speed(U8 port_id, S8 speed)
  66 {
  67         /* 1st arg:port id (0/1/2)        */
  68         /* 2nd arg:speed (-100 to 100)    */
  69         /* 3rd arg:mode (1:brake/0:float) */
  70
  71         /* As far as we tested the controllability of motor position for
  72          * precise real-time control applications (e.g. NXTway-GS, NXT GT)
  73          * brake mode seems to be better.
  74          */
  75         nxt_motor_set_speed(port_id, speed, 1);
  76 }
  77
  78 /**
  79  * set Servo Motor brake mode and PWM duty ratio
  80  *
  81  * @param port_id: NXT_PORT_A/NXT_PORT_B/NXT_PORT_C
  82  * @param mode: 0(float)/1(brake)
  83  * @param speed: PWM duty ratio (-100 to 100)
  84  */
  85 void ecrobot_set_motor_mode_speed(U8 port_id, S32 mode, S8 speed)
  86 {
  87         /* 1st arg:port id (0/1/2)        */
  88         /* 2nd arg:speed (-100 to 100)    */
  89         /* 3rd arg:mode (1:brake/0:float) */
  90         nxt_motor_set_speed(port_id, speed, mode);
  91 }
  92
  93 /**
  94  * get Servo Motor revolution in degree
  95  *
  96  * @param port_id: NXT_PORT_A/NXT_PORT_B/NXT_PORT_C
  97  * @return: motor revolution in degree
  98  */
  99 S32 ecrobot_get_motor_rev(U8 port_id)
 100 {
 101         return nxt_motor_get_count(port_id);
 102 }
 103
 104 /**
 105  * set Servo Motor revolution in degree
 106  *
 107  * @param port_id: NXT_PORT_A/NXT_PORT_B/NXT_PORT_C
 108  * @param rev: motor revolution in degree
 109  */
 110 void ecrobot_set_motor_rev(U8 port_id, S32 rev)
 111 {
 112         nxt_motor_set_count(port_id, rev);
 113 }
 114
 115
 116 /*==============================================================================
 117  * NXT A/D Sensors API
 118  *=============================================================================*/
 119
 120 /**
 121  * turn infra-red light on
 122  *
 123  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 124  */
 125 void ecrobot_set_light_sensor_active(U8 port_id)
 126 {
 127         set_digi0(port_id);
 128 }
 129
 130 /**
 131  * turn infra-red light off
 132  *
 133  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 134  */
 135 void ecrobot_set_light_sensor_inactive(U8 port_id)
 136 {
 137         unset_digi0(port_id);
 138 }
 139
 140 /**
 141  * get Light Sensor raw A/D data
 142  *
 143  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 144  * @return: A/D raw data(0 to 1023)
 145  */
 146 U16 ecrobot_get_light_sensor(U8 port_id)
 147 {
 148         return (U16)sensor_adc(port_id);
 149 }
 150
 151 /**
 152  * get Touch Sensor on/off status
 153  *
 154  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 155  * @return: 1(touched)/0(not touched)
 156  */
 157 U8 ecrobot_get_touch_sensor(U8 port_id)
 158 {
 159         return (sensor_adc(port_id) < 512);
 160 }
 161
 162 /**
 163  * get Sound Sensor raw A/D data
 164  *
 165  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 166  * @return: A/D raw data(0 to 1023)
 167  */
 168 U16 ecrobot_get_sound_sensor(U8 port_id)
 169 {
 170         return (U16)sensor_adc(port_id);
 171 }
 172
 173
 174 /*==============================================================================
 175  * NXT I2C API
 176  *=============================================================================*/
 177 /**
 178  * init a NXT sensor port for I2C communication
 179  *
 180  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 181  * @param type: LOWSPEED_9V/LOWSPEED
 182  */
 183 void ecrobot_init_i2c(U8 port_id, U8 type)
 184 {
 185         if (deviceStatus == DEVICE_NO_INIT)
 186         {
 187                 nxt_avr_set_input_power(port_id, type);
 188                 i2c_enable(port_id);
 189         }
 190 }
 191
 192 /**
 193  * send I2C data
 194  *
 195  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 196  * @param address: 0x01 to 0x7F
 197  *  Note that addresses are from 0x01 to 0x7F not
 198  *  even numbers from 0x02 to 0xFE as given in some I2C device specifications.
 199  *  They are 7-bit addresses not 8-bit addresses.
 200  * @param i2c_reg: I2C register e.g. 0x42
 201  * @param buf: buffer containing data to send
 202  * @param len: length of the data to send
 203  * @return: 1(success)/0(failure)
 204  */
 205 SINT ecrobot_send_i2c(U8 port_id, U32 address, SINT i2c_reg, U8 *buf, U32 len)
 206 {
 207         SINT ret;
 208
 209         ret = i2c_start_transaction(port_id, address, i2c_reg, len, buf, len, 1);
 210         if (ret != 0) return 0;
 211
 212         while(i2c_busy(port_id) != 0);
 213         return 1;
 214 }
 215
 216 /**
 217  * read I2C data
 218  *
 219  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 220  * @param address: 0x01 to 0x7F
 221  *  Note that addresses are from 0x01 to 0x7F not
 222  *  even numbers from 0x02 to 0xFE as given in some I2C device specifications.
 223  *  They are 7-bit addresses not 8-bit addresses.
 224  * @param i2c_reg: I2C register e.g. 0x42
 225  * @param buf: buffer to return data
 226  * @param len: length of the return data
 227  * @return: 1(success)/0(failure)
 228  */
 229 SINT ecrobot_read_i2c(U8 port_id, U32 address, SINT i2c_reg, U8 *buf, U32 len)
 230 {
 231         SINT ret;
 232
 233         ret = i2c_start_transaction(port_id, address, i2c_reg, len, buf, len, 0);
 234         if (ret != 0) return 0;
 235
 236         while(i2c_busy(port_id) != 0);
 237         return 1;
 238 }
 239
 240 /**
 241  * terminate a NXT sensor port used for I2C communication
 242  *
 243  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 244  */
 245 void ecrobot_term_i2c(U8 port_id)
 246 {
 247         i2c_disable(port_id);
 248 }
 249
 250
 251 /*==============================================================================
 252  * NXT Ultrasonic Sensor API
 253  *=============================================================================*/
 254 static S32 distance_state[4] = {-1,-1,-1,-1}; /* -1: sensor is not connected */
 255
 256 static S32 getDistance(void)
 257 {
 258         SINT i;
 259
 260         /*
 261          * if multiple Ultrasonic Sensors are connected to a NXT,
 262          * only the senosr measurement data which is connected to the smallest port id
 263          * can be monitored in LCD and logging data.
 264          */
 265         for (i = 0; i < 4; i++)
 266         {
 267                 if (distance_state[i] != -1)
 268                 {
 269                         return distance_state[i];
 270                 }
 271         }
 272         return -1;
 273 }
 274
 275 /**
 276  * init a NXT sensor port for Ultrasonic Sensor
 277  *
 278  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 279  */
 280 void ecrobot_init_sonar_sensor(U8 port_id)
 281 {
 282         ecrobot_init_i2c(port_id, LOWSPEED);
 283 }
 284
 285 /**
 286  * get Ultrasonic Sensor measurement data in cm
 287  *
 288  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 289  * @return: distance in cm (0 to 255), -1 (failure)
 290  *  NOTE that this API has one cycle delay between data acquisition request
 291  *  and actual data acquisition.
 292  */
 293 S32 ecrobot_get_sonar_sensor(U8 port_id)
 294 {
 295         static U8 data;
 296
 297         if (i2c_busy(port_id) == 0)
 298         {
 299                 distance_state[port_id] = (S32)data;
 300            /* i2c_start_transaction just triggers an I2C transaction,
 301                 * actual data transaction between ARM7 and a Ultrasonic
 302                 * Sensor is done by an ISR after this, so there is one cycle
 303                 * delay for consistent data acquistion
 304                 */
 305                 i2c_start_transaction(port_id,1,0x42,1,&data,1,0);
 306         }
 307
 308         return distance_state[port_id];
 309 }
 310
 311 /**
 312  * terminate I2C used for for Ultrasonic Sensor
 313  *
 314  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 315  */
 316 void ecrobot_term_sonar_sensor(U8 port_id)
 317 {
 318         distance_state[0] = -1;
 319         distance_state[1] = -1;
 320         distance_state[2] = -1;
 321         distance_state[3] = -1;
 322         i2c_disable(port_id);
 323 }
 324
 325
 326 /*==============================================================================
 327  * NXT HiTechnic Sensor API
 328  *=============================================================================*/
 329
 330 /**
 331  * get HiTechnic Gyro Sensor raw A/D data
 332  *
 333  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 334  * @return: A/D raw data(0 to 1023)
 335  */
 336 U16 ecrobot_get_gyro_sensor(U8 port_id)
 337 {
 338         return (U16)sensor_adc(port_id);
 339 }
 340
 341 /**
 342  * init a NXT port for Acceleration Sensor
 343  *
 344  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 345  */
 346 void ecrobot_init_accel_sensor(U8 port_id)
 347 {
 348         ecrobot_init_i2c(port_id, LOWSPEED);
 349 }
 350
 351 /**
 352  * get HiTechnic Acceleration Sensor data ([x,y,z] axes acceleration)
 353  *
 354  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 355  * @param buf: buffer to return the x/y/z axes accel data
 356  */
 357 void ecrobot_get_accel_sensor(U8 port_id, S16 *buf)
 358 {
 359          SINT i;
 360           /* support for multiple accel sensors in a NXT */
 361          static S16 accel_state[4][3];
 362          static U8 data[4][6];
 363         /*
 364          * data[0]: X axis upper 8 bits
 365          * data[1]: Y axis upper 8 bits
 366          * data[2]: Z axis upper 8 bits
 367          * data[3]: X axis lower 2 bits
 368          * data[4]: Y axis lower 2 bits
 369          * data[5]: Z axis lower 2 bits
 370          */
 371
 372         if (i2c_busy(port_id) == 0)
 373         {
 374                 for (i=0; i<3; i++)
 375                 {
 376                         accel_state[port_id][i] = (S16)data[port_id][i];
 377                         if (accel_state[port_id][i] > 127) accel_state[port_id][i] -= 256;
 378                         /* convert to 10 bit value */
 379                         accel_state[port_id][i] = (accel_state[port_id][i] << 2) | ((S16)data[port_id][i+3] & 0x0003);
 380                 }
 381            /* i2c_start_transaction just triggers an I2C transaction,
 382                 * actual data transaction between ARM7 and an Acceleration
 383                 * Sensor is done by an ISR after this, so there is one execution cycle
 384                 * delay for consistent data acquistion
 385                 */
 386                 i2c_start_transaction(port_id,1,0x42,1,&data[port_id][0],6,0);
 387         }
 388
 389         for (i=0; i<3; i++)
 390         {
 391                 buf[i] = accel_state[port_id][i];
 392         }
 393 }
 394
 395 /**
 396  * terminate I2C used for for Acceleration Sensor
 397  *
 398  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 399  */
 400 void ecrobot_term_accel_sensor(U8 port_id)
 401 {
 402         i2c_disable(port_id);
 403 }
 404
 405
 406 /*==============================================================================
 407  * RCX Sensors API
 408  *
 409  * According to LEGO Hardware Developer Kit, RCX sensors are categorized as
 410  * two types
 411  *   ACTIVE SENSOR:  RCX Light Sensor, RCX Rotation Sensor
 412  *   PASSIVE SENSOR: RCX Touch Sensor
 413  * ACTIVE SENSOR requires additional power source to drive the sensor
 414  * PASSIVE SENSOR is compatible with NXT A/D sensors
 415  *
 416  * Concept of these RCX Sensors API is contributed by Maurits Kooiman(Mansk).
 417  * see http://forums.nxtasy.org/index.php?showtopic=1540
 418  *=============================================================================*/
 419
 420 /**
 421  * supply power source for ACTIVE SENSORS
 422  *
 423  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 424  */
 425 void ecrobot_set_RCX_power_source(U8 port_id)
 426 {
 427         if (deviceStatus == DEVICE_NO_INIT)
 428         {
 429                 nxt_avr_set_input_power(port_id, LOWSPEED_9V);
 430         }
 431 }
 432
 433 /**
 434  * stop power source for ACTIVE SENSORS
 435  *
 436  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 437  */
 438 void ecrobot_term_RCX_power_source(U8 port_id)
 439 {
 440         nxt_avr_set_input_power(port_id, 0);
 441 }
 442
 443 /**
 444  * get RCX Sensor raw A/D data
 445  *
 446  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 447  * @return: raw A/D data (0 to 1023)
 448  */
 449 S16 ecrobot_get_RCX_sensor(U8 port_id)
 450 {
 451         return (S16)sensor_adc(port_id);
 452 }
 453
 454 /**
 455  * get RCX Touch Sensor on/off status
 456  *
 457  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
 458  * @return: 1(touched)/0(not touched)
 459  */
 460 U8 ecrobot_get_RCX_touch_sensor(U8 port_id)
 461 {
 462         return (sensor_adc(port_id) < 512);
 463 }
 464
 465
 466 /*==============================================================================
 467  * NXT internal status API
 468  *=============================================================================*/
 469 #define N_BTN_STATE 10
 470
 471 static nxt_inputs ecrobot_inputs;
 472
 473 /**
 474  * get battery voltage in mille volt
 475  *
 476  * @return: battery voltage in mV (e.g. 9000 = 9.000V)
 477  */
 478 U16 ecrobot_get_battery_voltage(void)
 479 {
 480         return (U16)ecrobot_inputs.battery_state;
 481 }
 482
 483 /**
 484  * get System tick in mille second
 485  *
 486  * @return: time in msec.
 487  *  NOTE that system tick is started when the NXT was turned on.
 488  *  not when application was started.
 489  */
 490 U32 ecrobot_get_systick_ms(void)
 491 {
 492         return systick_get_ms();
 493 }
 494
 495 /**
 496  * get ENTER buttons status
 497  *  NOTE that STOP and EXIT buttons are preserved by the system,
 498  *  so should not be disclosed to application.
 499  *
 500  * @return: 1(pressed)/0(not pressed)
 501  */
 502 U8 ecrobot_is_ENTER_button_pressed(void)
 503 {
 504         return ((ecrobot_get_button_state() & ENTER_PRESSED) == ENTER_PRESSED);
 505 }
 506
 507 /**
 508  * get RUN buttons status
 509  *  NOTE that STOP and EXIT buttons are preserved by the system,
 510  *  so should not be disclosed to application.
 511  *
 512  * @return: 1(pressed)/0(not pressed)
 513  */
 514 U8 ecrobot_is_RUN_button_pressed(void)
 515 {
 516         return ((ecrobot_get_button_state() & RUN_PRESSED) == RUN_PRESSED);
 517 }
 518
 519 U8 ecrobot_get_button_state(void)
 520 {
 521         return ecrobot_inputs.buttons_state;
 522 }
 523
 524 void ecrobot_poll_nxtstate(void)
 525 {
 526         static U8 buttons_states[N_BTN_STATE];
 527         static SINT buttons_i = 0;
 528         SINT i;
 529
 530         ecrobot_inputs.battery_state = battery_voltage();
 531
 532         /* button debouncer */
 533         buttons_states[buttons_i++] = buttons_get();
 534         if (buttons_i == N_BTN_STATE) buttons_i = 0;
 535         for (i = 1; i < N_BTN_STATE; i++)
 536         {
 537                 if (buttons_states[i-1] != buttons_states[i])
 538                 {
 539                         break;
 540                 }
 541                 else if (i == N_BTN_STATE-1)
 542         {
 543                         ecrobot_inputs.buttons_state = buttons_states[i];
 544         }
 545         }
 546 }
 547
 548 void ecrobot_setDeviceInitialized(void)
 549 {
 550         if (deviceStatus == DEVICE_NO_INIT)
 551         {
 552                 deviceStatus = DEVICE_INITIALIZED;
 553         }
 554 }
 555
 556
 557 /*==============================================================================
 558  * NXT Data Logging API for NXT GamePad
 559  *=============================================================================*/
 560 static S16 adc[4]; /* used for ecrobot_adc_data_monitor */
 561
 562 /**
 563  * data logging API used with NXT GamePad
 564  *
 565  * @param data1: user configurable data to be logged
 566  * @param data2: user configurable data to be logged
 567  */
 568 void ecrobot_bt_data_logger(S8 data1, S8 data2)
 569 {
 570         U8 data_log_buffer[32];
 571
 572         *((U32 *)(&data_log_buffer[0]))  = (U32)systick_get_ms();
 573         *(( S8 *)(&data_log_buffer[4]))  =  (S8)data1;
 574         *(( S8 *)(&data_log_buffer[5]))  =  (S8)data2;
 575         *((U16 *)(&data_log_buffer[6]))  = (U16)ecrobot_inputs.battery_state;
 576         *((S32 *)(&data_log_buffer[8]))  = (S32)nxt_motor_get_count(0);
 577         *((S32 *)(&data_log_buffer[12])) = (S32)nxt_motor_get_count(1);
 578         *((S32 *)(&data_log_buffer[16])) = (S32)nxt_motor_get_count(2);
 579         *((S16 *)(&data_log_buffer[20])) = (S16)sensor_adc(0);
 580         *((S16 *)(&data_log_buffer[22])) = (S16)sensor_adc(1);
 581         *((S16 *)(&data_log_buffer[24])) = (S16)sensor_adc(2);
 582         *((S16 *)(&data_log_buffer[26])) = (S16)sensor_adc(3);
 583         *((S32 *)(&data_log_buffer[28])) = (S32)getDistance();
 584
 585         ecrobot_send_bt_packet(data_log_buffer, 32);
 586 }
 587
 588 /**
 589  * data logging API used with NXT GamePad
 590  *
 591  * @param data1: user configurable data to be logged
 592  * @param data2: user configurable data to be logged
 593  * @param adc1:  user configurable data to be logged
 594  * @param adc2:  user configurable data to be logged
 595  * @param adc3:  user configurable data to be logged
 596  * @param adc4:  user configurable data to be logged
 597  */
 598 void ecrobot_bt_adc_data_logger(S8 data1, S8 data2, S16 adc1, S16 adc2, S16 adc3, S16 adc4)
 599 {
 600         U8 data_log_buffer[32];
 601
 602         *((U32 *)(&data_log_buffer[0]))  = (U32)systick_get_ms();
 603         *(( S8 *)(&data_log_buffer[4]))  =  (S8)data1;
 604         *(( S8 *)(&data_log_buffer[5]))  =  (S8)data2;
 605         *((U16 *)(&data_log_buffer[6]))  = (U16)ecrobot_inputs.battery_state;
 606         *((S32 *)(&data_log_buffer[8]))  = (S32)nxt_motor_get_count(0);
 607         *((S32 *)(&data_log_buffer[12])) = (S32)nxt_motor_get_count(1);
 608         *((S32 *)(&data_log_buffer[16])) = (S32)nxt_motor_get_count(2);
 609         *((S16 *)(&data_log_buffer[20])) = (S16)adc1;
 610         *((S16 *)(&data_log_buffer[22])) = (S16)adc2;
 611         *((S16 *)(&data_log_buffer[24])) = (S16)adc3;
 612         *((S16 *)(&data_log_buffer[26])) = (S16)adc4;
 613         *((S32 *)(&data_log_buffer[28])) = (S32)getDistance();
 614         adc[0] = adc1;
 615         adc[1] = adc2;
 616         adc[2] = adc3;
 617         adc[3] = adc4;
 618
 619         ecrobot_send_bt_packet(data_log_buffer, 32);
 620 }
 621
 622
 623 /*==============================================================================
 624  * NXT LCD display API
 625  *=============================================================================*/
 626
 627 /**
 628  * convert a BMP file to an array data for LCD display
 629  *
 630  * @param file: monochrome BMP file data
 631  * @param lcd: data array to be drawn in LCD
 632  * @param width: pixel width of the BMP file (max. 100)
 633  * @param height: pixel height of the BMP file (max. 64)
 634  * @return: 1(success)/-1(failure)
 635  */
 636 SINT ecrobot_bmp2lcd(const CHAR *file, U8 *lcd, S32 width, S32 height)
 637 {
 638         SINT bmp_line, bmp_line_alignment;
 639         SINT bmp_row, bmp_col;
 640         SINT lcd_row;
 641         SINT lcd_pos;
 642         SINT bmp_bit_pos;
 643         SINT lcd_bit_pos;
 644         SINT bits;
 645         U8 bmp_data;
 646         BMP *bmp = (BMP *)file;
 647
 648         /* check a BMP file header information */
 649         if (bmp->fileHeader.type != BM_TYPE)  /* Windows */
 650                 return -1;
 651         if ((bmp->infoHeader.width > NXT_LCD_WIDTH) ||
 652                 (bmp->infoHeader.width < 1) ||
 653                 (bmp->infoHeader.width != width))
 654                 return -1;
 655         if ((bmp->infoHeader.height > NXT_LCD_DEPTH * 8) ||
 656                 (bmp->infoHeader.height < 1) ||
 657                 (bmp->infoHeader.height != height))
 658                 return -1;
 659         if (bmp->infoHeader.bits != 1)        /* monochrome bmp */
 660                 return -1;
 661         if (bmp->infoHeader.compression != 0) /* no compression */
 662                 return -1;
 663
 664         /*
 665          * Specifications of a monochrome bmp file
 666          *
 667          * - each bit represents a pixel.
 668          * - each line has 4 bytes alignment.
 669          *   (100 pixels data is saved in 100/8 = 12 + 1 + 3 = 16 byte)
 670          * - data order in a BMP file:
 671      *   (0,n)---------------->(m,n)
 672      *        ---------------->
 673      *        ---------------->
 674      *        ---------------->
 675      *   (0,0)---------------->(m,0)
 676          */
 677
 678         /* calculate line alignment */
 679         bmp_line = bmp->infoHeader.width / 8;
 680         if (bmp->infoHeader.width % 8)
 681         {
 682                 bmp_line++;
 683         }
 684
 685         if ((bmp_line % 4) != 0)
 686         {
 687                 bmp_line_alignment = ((bmp_line / 4) + 1) * 4;
 688         }
 689         else
 690         {
 691                 bmp_line_alignment = bmp_line;
 692         }
 693
 694         /* convert BMP data to LCD array data */
 695         for (bmp_row = 0; bmp_row < bmp->infoHeader.height; bmp_row++)
 696         {
 697                 lcd_row = NXT_LCD_DEPTH - ((bmp_row / 8) + 1);
 698                 lcd_bit_pos = bmp_row % 8;
 699                 bits = 0;
 700                 for(bmp_col = 0; bmp_col < bmp_line; bmp_col++)
 701                 {
 702                         bmp_data = file[(bmp_row * bmp_line_alignment) + bmp_col + bmp->fileHeader.offset];
 703                         lcd_pos = (lcd_row * NXT_LCD_WIDTH) + (bmp_col * 8);
 704                         for (bmp_bit_pos = 0; ((bmp_bit_pos < 8) && (bits < bmp->infoHeader.width)); bmp_bit_pos++, bits++)
 705                         {
 706                                 if (!(bmp_data & (0x01 << (7 - bmp_bit_pos))))
 707                                 {
 708                                         lcd[lcd_pos + bmp_bit_pos] |= (0x80 >> lcd_bit_pos);
 709                                 }
 710                                 else
 711                                 {
 712                                         lcd[lcd_pos + bmp_bit_pos] &= (~0x80 >> lcd_bit_pos);
 713                                 }
 714                         }
 715                 }
 716         }
 717         return 1;
 718 }
 719
 720 void ecrobot_show_int(S32 var)
 721 {
 722         display_clear(0);
 723
 724         display_goto_xy(0, 7);
 725         display_string("VAR: ");
 726         display_int(var, 0);
 727
 728         display_update();
 729 }
 730
 731 void ecrobot_debug(UINT var)
 732 {
 733         display_clear(0);
 734
 735         display_goto_xy(0, 7);
 736         display_string("DBG: ");
 737         display_int(var, 0);
 738
 739         display_update();
 740 }
 741
 742 void ecrobot_debug1(UINT var1, UINT var2, UINT var3)
 743 {
 744         display_clear(0);
 745
 746         display_goto_xy(0, 1);
 747         display_string("VAR1: ");
 748         display_int(var1, 0);
 749
 750         display_goto_xy(0, 2);
 751         display_string("VAR2: ");
 752         display_int(var2, 0);
 753
 754         display_goto_xy(0, 3);
 755         display_string("VAR3: ");
 756         display_int(var3, 0);
 757
 758         display_update();
 759 }
 760
 761 void ecrobot_debug2(UINT var1, UINT var2, UINT var3)
 762 {
 763         display_clear(0);
 764
 765         display_goto_xy(0, 4);
 766         display_string("VAR4: ");
 767         display_int(var1, 0);
 768
 769         display_goto_xy(0, 5);
 770         display_string("VAR5: ");
 771         display_int(var2, 0);
 772
 773         display_goto_xy(0, 6);
 774         display_string("VAR6: ");
 775         display_int(var3, 0);
 776
 777         display_update();
 778 }
 779
 780 void ecrobot_status_monitor(const CHAR *target_name)
 781 {
 782         display_clear(0);
 783
 784         display_goto_xy(0, 0);
 785         display_string(target_name);
 786
 787         display_goto_xy(0, 1);
 788         display_string("TIME:");
 789         display_unsigned(systick_get_ms()/1000, 0);
 790
 791         display_goto_xy(0, 2);
 792         display_string("BATT:");
 793         display_unsigned(ecrobot_inputs.battery_state/100, 0);
 794
 795         display_goto_xy(0, 3);
 796         display_string("REV: ");
 797         display_int(nxt_motor_get_count(0), 0);
 798         display_int(nxt_motor_get_count(1), 6);
 799
 800         display_goto_xy(0, 4);
 801         display_string("     ");
 802         display_int(nxt_motor_get_count(2), 0);
 803
 804         display_goto_xy(0, 5);
 805         display_string("ADC: ");
 806         display_unsigned(sensor_adc(0), 0);
 807         display_unsigned(sensor_adc(1), 5);
 808
 809         display_goto_xy(0, 6);
 810         display_string("     ");
 811         display_unsigned(sensor_adc(2), 0);
 812         display_unsigned(sensor_adc(3), 5);
 813
 814         display_goto_xy(0, 7);
 815         display_string("BT/DST: ");
 816         if (ecrobot_get_bt_status() == BT_STREAM)
 817         {
 818                 display_unsigned(1, 0);
 819         }
 820         else
 821         {
 822                 display_unsigned(0, 0);
 823         }
 824         display_int(getDistance(), 5);
 825
 826         display_update();
 827 }
 828
 829 /* This function is used to display user selected sensor values */
 830 void ecrobot_adc_data_monitor(const CHAR *target_name)
 831 {
 832         display_clear(0);
 833
 834         display_goto_xy(0, 0);
 835         display_string(target_name);
 836
 837         display_goto_xy(0, 1);
 838         display_string("TIME:");
 839         display_unsigned(systick_get_ms()/1000, 0);
 840
 841         display_goto_xy(0, 2);
 842         display_string("BATT:");
 843         display_unsigned(ecrobot_inputs.battery_state/100, 0);
 844
 845         display_goto_xy(0, 3);
 846         display_string("REV: ");
 847         display_int(nxt_motor_get_count(0), 0);
 848         display_int(nxt_motor_get_count(1), 6);
 849
 850         display_goto_xy(0, 4);
 851         display_string("     ");
 852         display_int(nxt_motor_get_count(2), 0);
 853
 854         display_goto_xy(0, 5);
 855         display_string("ADC1/2:");
 856         display_int(adc[0], 0);
 857         display_int(adc[1], 5);
 858
 859         display_goto_xy(0, 6);
 860         display_string("ADC3/4:");
 861         display_int(adc[2], 0);
 862         display_int(adc[3], 5);
 863
 864         display_goto_xy(0, 7);
 865         display_string("BT/DST: ");
 866         if (ecrobot_get_bt_status() == BT_STREAM)
 867         {
 868                 display_unsigned(1, 0);
 869         }
 870         else
 871         {
 872                 display_unsigned(0, 0);
 873         }
 874         display_int(getDistance(), 5);
 875
 876         display_update();
 877 }
 878
 879
 880 /*==============================================================================
 881  * NXT Sound API
 882  *=============================================================================*/
 883
 884 /**
 885  * play a tone(wrapper of leJOS sound tone api)
 886  *
 887  * @param freq: soundfrequency in Hz (31 to 2100)
 888  * @param ms: sound duration in centisecond (10 - 256)
 889  * @vol: sound volume (0 to 100)
 890  * @return 1 (always)
 891  */
 892 SINT ecrobot_sound_tone(U32 freq, U32 ms, U32 vol)
 893 {
 894         sound_freq_vol(freq, ms, vol);
 895         return 1;
 896 }
 897
 898 /**
 899  * play a WAV file (support for only 8bit monoral PCM)
 900  *
 901  * @param file: WAV file data
 902  *  NOTE that supports only 8bit monoral PCM
 903  * @param length: length of WAV file
 904  * @param freq: sampling frequency -1(inherit the WAV file original)/2000 to 22050Hz
 905  * @param vol: sound volume(0 to 100)
 906  * @return: 1(success)/0(sound resource is busy)/-1(failure)
 907  */
 908 SINT ecrobot_sound_wav(const CHAR *file, U32 length, S32 freq, U32 vol)
 909 {
 910         WAV *wav = (WAV *)file;
 911
 912         /* check sound resource is free */
 913         if (sound_get_time() > 0)
 914                 return 0;
 915
 916         /* check length of a file */
 917         if (length < WAV_HDR_SIZE)
 918                 return -1;
 919
 920         /* check a WAV file header information */
 921         if (wav->riff.chunkID != RIFF_CHUNK_ID)
 922                 return -1;
 923         if (wav->riff.format != RIFF_FORMAT)
 924                 return -1;
 925         if (wav->fmt.chunkID != FMT_CHUNK_ID)
 926                 return -1;
 927         if (wav->fmt.audioFormat != 0x0001) /* PCM */
 928                 return -1;
 929         if (wav->fmt.numChannels != 0x0001) /* mono channel */
 930                 return -1;
 931         if (wav->fmt.bitsPerSample != 0x0008) /* 8bit */
 932                 return -1;
 933
 934         /* in case of freq < 0, freq(sample rate) is inherited from a WAV file */
 935         if (freq < 0)
 936         {
 937                 freq = wav->fmt.sampleRate;
 938         }
 939
 940         /* read wav data. Currently, supported PCM file types are
 941          * linear PCM(data chunkID is "data" and "fact") and non-linear PCM.
 942          */
 943         if (wav->data.chunkID == DATA_CHUNK_ID)
 944         {
 945                 /* linear PCM */
 946                 sound_play_sample(wav->data.data, wav->data.chunkSize, (U32)freq, vol);
 947         }
 948         else
 949         {
 950                 WAV_FACT *wav_fact = (WAV_FACT *)file;
 951                 if (wav_fact->data.chunkID == FACT_CHUNK_ID && wav_fact->data.data_chunkID == DATA_CHUNK_ID)
 952                 {
 953                         /* linear PCM with "fact" chunk ID */
 954                         sound_play_sample(wav_fact->data.data, wav_fact->data.data_chunkSize, (U32)freq, vol);
 955                 }
 956                 else
 957                 {
 958                         WAV_NL *wav_nl = (WAV_NL *)file;
 959                         if (wav_nl->data.chunkID == DATA_CHUNK_ID)
 960                         {
 961                                 /* non linear PCM */
 962                                 sound_play_sample(wav_nl->data.data, wav_nl->data.chunkSize, (U32)freq, vol);
 963                         }
 964                         else
 965                         {
 966                                 return -1;
 967                         }
 968                 }
 969         }
 970         return 1;
 971 }
 972
 973
 974 /*==============================================================================
 975  * NXT USB API
 976  *=============================================================================*/
 977 static U8 usb_reset_flag = 0;
 978
 979 /**
 980  * reset USB.
 981  * This API can be invoked in either ecrobot_device_initialize
 982  * and run-time application
 983  */
 984 void ecrobot_init_usb(void)
 985 {
 986         /* invoked in ecrobot_device_initialize */
 987         if(!get_OS_flag())
 988         {
 989                 /* invoked only for the first time */
 990                 if (deviceStatus == DEVICE_NO_INIT)
 991                 {
 992                         udp_reset();
 993                         usb_reset_flag = 1;
 994                         add_status_info(USB_INITIALIZED);
 995                 }
 996         }
 997         /* invoked in run-time application */
 998         else
 999         {
1000                 udp_reset();
1001                 usb_reset_flag = 1;
1002         }
1003 }
1004
1005 /**
1006  * read USB data from host
1007  *
1008  * @param buf: buffer to return data
1009  * @param len: length of the return data
1010  * @return: 1(success)/0(failure)
1011  */
1012 SINT ecrobot_read_usb(U8 *buf, U32 len)
1013 {
1014         if (usb_reset_flag != 1)
1015                 return 0;
1016
1017         if (len > MAX_USB_DATA_SIZE)
1018                 return 0;
1019
1020         return udp_read(buf, len);
1021 }
1022
1023 /**
1024  * send USB data to host
1025  *
1026  * @param buf: buffer containing data to send
1027  * @param len: length of the data to send
1028  * @return: 1(success)/0(failure)
1029  */
1030 SINT ecrobot_send_usb(U8 *buf, U32 len)
1031 {
1032         if (usb_reset_flag != 1)
1033                 return 0;
1034
1035         if (len > MAX_USB_DATA_SIZE)
1036                 return 0;
1037
1038         udp_write(buf, len);
1039         return len;
1040 }
1041
1042 /**
1043  * close a USB connection. (currently it's just reset the flag)
1044  */
1045 void ecrobot_term_usb(void)
1046 {
1047         usb_reset_flag = 0;
1048         udp_close(0);
1049 }