/*****************************************************************************
* FILE: ecrobot_interface.c
*
- * COPYRIGHT 2007 Takashi Chikamasa <takashic@cybernet.co.jp>
+ * COPYRIGHT 2008 Takashi Chikamasa <takashic@cybernet.co.jp>
*
- * DESCRIPTION:
- * This is a glue code to execute Embedded Coder Robot NXT generated code
- * by using LEJOS NXJ.
+ * <About leJOS NXJ>
+ * leJOS NXJ is a full firmware replacement of LEGO Mindstorms NXT and
+ * designed for Java programming environment for the NXT
+ * ( For more detailed information, please see: http://lejos.sourceforge.net/ )
+ * In the leJOS NXJ distribution, C source files for NXT platform layer is also
+ * included besides with the Java VM. The platform C source code is well
+ * structured, comprehensive, and achieved higher performance than the LEGO's
+ * one. Therefore, leJOS NXJ (platform) is also the best GCC based C/C++
+ * development platform for NXT.
*
- * <About LEJOS NXJ>
- * LEJOS NXJ is a full firmware replacement of LEGO Mindstorms NXT and
- * designed for Java programming environment for the NXT
- * ( For more detailed information, please see: http://lejos.sourceforge.net/ )
- * In the LEJOS NXJ distribution, C source files for NXT platform layer is also
- * included besides with the Java VM. The platform C source code is well
- * structured, comprehensive, and achieved higher performance than the LEGO's
- * one. Therefore, LEJOS NXJ (platform) is also the best GCC based C/C++
- * development platform for NXT.
+ * The contents of this file are subject to the Mozilla Public License
+ * Version 1.0 (the "License"); you may not use this file except in
+ * compliance with the License. You may obtain a copy of the License at
+ * http://www.mozilla.org/MPL/
*
- * The contents of this file are subject to the Mozilla Public License
- * Version 1.0 (the "License"); you may not use this file except in
- * compliance with the License. You may obtain a copy of the License at
- * http://www.mozilla.org/MPL/
+ * Software distributed under the License is distributed on an "AS IS"
+ * basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
+ * License for the specific language governing rights and limitations
+ * under the License.
*
- * Software distributed under the License is distributed on an "AS IS"
- * basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
- * License for the specific language governing rights and limitations
- * under the License.
+ * The Original Code is TinyVM code, first released March 6, 2000,
+ * later released as leJOS on September 23, 2000.
*
- * The Original Code is TinyVM code, first released March 6, 2000,
- * later released as leJOS on September 23, 2000.
+ * The Initial Developer of the Original Code is Jose H. Solorzano.
*
- * The Initial Developer of the Original Code is Jose H. Solorzano.
+ * Contributor(s): see leJOS NXJ ACKNOWLEDGEMENTS .
*
- * Contributor(s): see LEJOS NXJ ACKNOWLEDGEMENTS .
*
- ******************************************************************************/
+ * <About TOPPERS OSEK>
+ * TOPPERS OSEK is an open source OSEK kernel and developed by TOPPERS project.
+ * TOPPERS(Toyohashi OPen Platform for Embedded Real-time Systems) has been managed
+ * by a Non Profit Organization founded in Sep. 2003 and has been led by Professor
+ * Hiroaki Takada of Nagoya University in Japan.
+ *
+ * TOPPERS OSEK program is covered by the TOPPERS License as published
+ * by the TOPPERS PROJECT (http://www.toppers.jp/en/index.html).
+ *
+ *****************************************************************************/
#include <stddef.h>
#include <string.h>
-/* ECRobot NXT */
+#include "ecrobot_base.h"
+#include "ecrobot_private.h"
#include "ecrobot_interface.h"
+static U8 deviceStatus = DEVICE_NO_INIT;
-/*
- * gMakeRequest is defined in interpreter.c for Java VM,
- * but not used for platform functions, this is dummy
- */
-volatile boolean gMakeRequest;
-
-volatile U8 gDevice_status;
/*==============================================================================
- * ECRobot NXT Servo Motor API
+ * NXT Servo Motor API
*=============================================================================*/
-/*
- * Set Servo Motor PWM duty ratio
+
+/**
+ * set Servo Motor PWM duty ratio
+ *
+ * @param port_id: NXT_PORT_A/NXT_PORT_B/NXT_PORT_C
+ * @param speed: PWM duty ration (-100 to 100)
*/
void ecrobot_set_motor_speed(U8 port_id, S8 speed)
{
/* 1st arg:port id (0/1/2) */
/* 2nd arg:speed (-100 to 100) */
- /* 3rd arg:mode (0:brake/1:float) */
+ /* 3rd arg:mode (1:brake/0:float) */
+
+ /* As far as we tested the controllability of motor position for
+ * precise real-time control applications (e.g. NXTway-GS, NXT GT)
+ * brake mode seems to be better.
+ */
nxt_motor_set_speed(port_id, speed, 1);
}
-/*
- * Get Servo Motor revolution in degree
+/**
+ * set Servo Motor brake mode and PWM duty ratio
+ *
+ * @param port_id: NXT_PORT_A/NXT_PORT_B/NXT_PORT_C
+ * @param mode: 0(float)/1(brake)
+ * @param speed: PWM duty ratio (-100 to 100)
+ */
+void ecrobot_set_motor_mode_speed(U8 port_id, S32 mode, S8 speed)
+{
+ /* 1st arg:port id (0/1/2) */
+ /* 2nd arg:speed (-100 to 100) */
+ /* 3rd arg:mode (1:brake/0:float) */
+ nxt_motor_set_speed(port_id, speed, mode);
+}
+
+/**
+ * get Servo Motor revolution in degree
+ *
+ * @param port_id: NXT_PORT_A/NXT_PORT_B/NXT_PORT_C
+ * @return: motor revolution in degree
*/
S32 ecrobot_get_motor_rev(U8 port_id)
{
return nxt_motor_get_count(port_id);
}
+/**
+ * set Servo Motor revolution in degree
+ *
+ * @param port_id: NXT_PORT_A/NXT_PORT_B/NXT_PORT_C
+ * @param rev: motor revolution in degree
+ */
+void ecrobot_set_motor_rev(U8 port_id, S32 rev)
+{
+ nxt_motor_set_count(port_id, rev);
+}
+
+
/*==============================================================================
- * ECRobot NXT A/D Sensors API
+ * NXT A/D Sensors API
*=============================================================================*/
-/*
- * Turn infra-red light on
+
+/**
+ * turn infra-red light on
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
*/
void ecrobot_set_light_sensor_active(U8 port_id)
{
- if (gDevice_status == DEVICE_NO_INIT)
- {
- set_digi0(port_id);
- }
+ set_digi0(port_id);
}
-/*
- * Turn infra-red light off
+/**
+ * turn infra-red light off
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
*/
void ecrobot_set_light_sensor_inactive(U8 port_id)
{
unset_digi0(port_id);
}
-/*
- * Get Light Sensor raw A/D data
+/**
+ * get Light Sensor raw A/D data
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ * @return: A/D raw data(0 to 1023)
*/
U16 ecrobot_get_light_sensor(U8 port_id)
{
return (U16)sensor_adc(port_id);
}
-/*
- * Get Touch Sensor on/off status
+/**
+ * get Touch Sensor on/off status
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ * @return: 1(touched)/0(not touched)
*/
U8 ecrobot_get_touch_sensor(U8 port_id)
{
return (sensor_adc(port_id) < 512);
}
-/*
- * Get Sound Sensor raw A/D data
+/**
+ * get Sound Sensor raw A/D data
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ * @return: A/D raw data(0 to 1023)
*/
U16 ecrobot_get_sound_sensor(U8 port_id)
{
return (U16)sensor_adc(port_id);
}
+
/*==============================================================================
- * ECRobot NXT Digital Sensors API
+ * NXT I2C API
*=============================================================================*/
-/* Ultrasonic Sensor measurement data */
-static S32 distance_state;
-
-/*
- * Init an I2C for Ultrasonic Sensor
+/**
+ * init a NXT sensor port for I2C communication
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ * @param type: LOWSPEED_9V/LOWSPEED
*/
-void ecrobot_init_sonar_sensor(U8 port_id)
+void ecrobot_init_i2c(U8 port_id, U8 type)
{
- if (gDevice_status == DEVICE_NO_INIT)
+ if (deviceStatus == DEVICE_NO_INIT)
{
- distance_state = -1;
- nxt_avr_set_input_power(port_id,2);
+ nxt_avr_set_input_power(port_id, type);
i2c_enable(port_id);
}
}
-/*
- * Get Ultrasonic Sensor measurement data in cm via I2C
+/**
+ * send I2C data
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ * @param address: 0x01 to 0x7F
+ * Note that addresses are from 0x01 to 0x7F not
+ * even numbers from 0x02 to 0xFE as given in some I2C device specifications.
+ * They are 7-bit addresses not 8-bit addresses.
+ * @param i2c_reg: I2C register e.g. 0x42
+ * @param buf: buffer containing data to send
+ * @param len: length of the data to send
+ * @return: 1(success)/0(failure)
+ */
+SINT ecrobot_send_i2c(U8 port_id, U32 address, SINT i2c_reg, U8 *buf, U32 len)
+{
+ SINT ret;
+
+ ret = i2c_start_transaction(port_id, address, i2c_reg, len, buf, len, 1);
+ if (ret != 0) return 0;
+
+ while(i2c_busy(port_id) != 0);
+ return 1;
+}
+
+/**
+ * read I2C data
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ * @param address: 0x01 to 0x7F
+ * Note that addresses are from 0x01 to 0x7F not
+ * even numbers from 0x02 to 0xFE as given in some I2C device specifications.
+ * They are 7-bit addresses not 8-bit addresses.
+ * @param i2c_reg: I2C register e.g. 0x42
+ * @param buf: buffer to return data
+ * @param len: length of the return data
+ * @return: 1(success)/0(failure)
+ */
+SINT ecrobot_read_i2c(U8 port_id, U32 address, SINT i2c_reg, U8 *buf, U32 len)
+{
+ SINT ret;
+
+ ret = i2c_start_transaction(port_id, address, i2c_reg, len, buf, len, 0);
+ if (ret != 0) return 0;
+
+ while(i2c_busy(port_id) != 0);
+ return 1;
+}
+
+/**
+ * terminate a NXT sensor port used for I2C communication
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ */
+void ecrobot_term_i2c(U8 port_id)
+{
+ i2c_disable(port_id);
+}
+
+
+/*==============================================================================
+ * NXT Ultrasonic Sensor API
+ *=============================================================================*/
+static S32 distance_state[4] = {-1,-1,-1,-1}; /* -1: sensor is not connected */
+
+static S32 getDistance(void)
+{
+ SINT i;
+
+ /*
+ * if multiple Ultrasonic Sensors are connected to a NXT,
+ * only the senosr measurement data which is connected to the smallest port id
+ * can be monitored in LCD and logging data.
+ */
+ for (i = 0; i < 4; i++)
+ {
+ if (distance_state[i] != -1)
+ {
+ return distance_state[i];
+ }
+ }
+ return -1;
+}
+
+/**
+ * init a NXT sensor port for Ultrasonic Sensor
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ */
+void ecrobot_init_sonar_sensor(U8 port_id)
+{
+ ecrobot_init_i2c(port_id, LOWSPEED);
+}
+
+/**
+ * get Ultrasonic Sensor measurement data in cm
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ * @return: distance in cm (0 to 255), -1 (failure)
+ * NOTE that this API has one cycle delay between data acquisition request
+ * and actual data acquisition.
*/
S32 ecrobot_get_sonar_sensor(U8 port_id)
{
- static U8 distance;
+ static U8 data;
if (i2c_busy(port_id) == 0)
{
+ distance_state[port_id] = (S32)data;
/* i2c_start_transaction just triggers an I2C transaction,
- * actual data transaction between ARM and AVR in Ultrasonic
- * Sensor is done by an ISR after this, so there is one cycle
+ * actual data transaction between ARM7 and a Ultrasonic
+ * Sensor is done by an ISR after this, so there is one cycle
* delay for consistent data acquistion
*/
- distance_state = (S32)distance;
- i2c_start_transaction(port_id,1,0x42,1,&distance,1,0);
+ i2c_start_transaction(port_id,1,0x42,1,&data,1,0);
}
- return distance_state;
+ return distance_state[port_id];
}
-/*
- * Terminate an I2C for Ultrasonic Sensor
+/**
+ * terminate I2C used for for Ultrasonic Sensor
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
*/
void ecrobot_term_sonar_sensor(U8 port_id)
{
+ distance_state[0] = -1;
+ distance_state[1] = -1;
+ distance_state[2] = -1;
+ distance_state[3] = -1;
+ i2c_disable(port_id);
+}
+
+
+/*==============================================================================
+ * NXT HiTechnic Sensor API
+ *=============================================================================*/
+
+/**
+ * get HiTechnic Gyro Sensor raw A/D data
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ * @return: A/D raw data(0 to 1023)
+ */
+U16 ecrobot_get_gyro_sensor(U8 port_id)
+{
+ return (U16)sensor_adc(port_id);
+}
+
+/**
+ * init a NXT port for Acceleration Sensor
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ */
+void ecrobot_init_accel_sensor(U8 port_id)
+{
+ ecrobot_init_i2c(port_id, LOWSPEED);
+}
+
+/**
+ * get HiTechnic Acceleration Sensor data ([x,y,z] axes acceleration)
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ * @param buf: buffer to return the x/y/z axes accel data
+ */
+void ecrobot_get_accel_sensor(U8 port_id, S16 *buf)
+{
+ SINT i;
+ /* support for multiple accel sensors in a NXT */
+ static S16 accel_state[4][3];
+ static U8 data[4][6];
+ /*
+ * data[0]: X axis upper 8 bits
+ * data[1]: Y axis upper 8 bits
+ * data[2]: Z axis upper 8 bits
+ * data[3]: X axis lower 2 bits
+ * data[4]: Y axis lower 2 bits
+ * data[5]: Z axis lower 2 bits
+ */
+
+ if (i2c_busy(port_id) == 0)
+ {
+ for (i=0; i<3; i++)
+ {
+ accel_state[port_id][i] = (S16)data[port_id][i];
+ if (accel_state[port_id][i] > 127) accel_state[port_id][i] -= 256;
+ /* convert to 10 bit value */
+ accel_state[port_id][i] = (accel_state[port_id][i] << 2) | ((S16)data[port_id][i+3] & 0x0003);
+ }
+ /* i2c_start_transaction just triggers an I2C transaction,
+ * actual data transaction between ARM7 and an Acceleration
+ * Sensor is done by an ISR after this, so there is one execution cycle
+ * delay for consistent data acquistion
+ */
+ i2c_start_transaction(port_id,1,0x42,1,&data[port_id][0],6,0);
+ }
+
+ for (i=0; i<3; i++)
+ {
+ buf[i] = accel_state[port_id][i];
+ }
+}
+
+/**
+ * terminate I2C used for for Acceleration Sensor
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ */
+void ecrobot_term_accel_sensor(U8 port_id)
+{
i2c_disable(port_id);
}
+
/*==============================================================================
- * ECRobot NXT internal status API
+ * RCX Sensors API
+ *
+ * According to LEGO Hardware Developer Kit, RCX sensors are categorized as
+ * two types
+ * ACTIVE SENSOR: RCX Light Sensor, RCX Rotation Sensor
+ * PASSIVE SENSOR: RCX Touch Sensor
+ * ACTIVE SENSOR requires additional power source to drive the sensor
+ * PASSIVE SENSOR is compatible with NXT A/D sensors
+ *
+ * Concept of these RCX Sensors API is contributed by Maurits Kooiman(Mansk).
+ * see http://forums.nxtasy.org/index.php?showtopic=1540
*=============================================================================*/
-/* Buttons and Battery voltage */
+
+/**
+ * supply power source for ACTIVE SENSORS
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ */
+void ecrobot_set_RCX_power_source(U8 port_id)
+{
+ if (deviceStatus == DEVICE_NO_INIT)
+ {
+ nxt_avr_set_input_power(port_id, LOWSPEED_9V);
+ }
+}
+
+/**
+ * stop power source for ACTIVE SENSORS
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ */
+void ecrobot_term_RCX_power_source(U8 port_id)
+{
+ nxt_avr_set_input_power(port_id, 0);
+}
+
+/**
+ * get RCX Sensor raw A/D data
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ * @return: raw A/D data (0 to 1023)
+ */
+S16 ecrobot_get_RCX_sensor(U8 port_id)
+{
+ return (S16)sensor_adc(port_id);
+}
+
+/**
+ * get RCX Touch Sensor on/off status
+ *
+ * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4
+ * @return: 1(touched)/0(not touched)
+ */
+U8 ecrobot_get_RCX_touch_sensor(U8 port_id)
+{
+ return (sensor_adc(port_id) < 512);
+}
+
+
+/*==============================================================================
+ * NXT internal status API
+ *=============================================================================*/
+#define N_BTN_STATE 10
+
static nxt_inputs ecrobot_inputs;
-/*
- * Get Battery Voltage in mille volt
+/**
+ * get battery voltage in mille volt
+ *
+ * @return: battery voltage in mV (e.g. 9000 = 9.000V)
*/
U16 ecrobot_get_battery_voltage(void)
{
return (U16)ecrobot_inputs.battery_state;
}
-/*
- * Get System tick in mille second
+/**
+ * get System tick in mille second
+ *
+ * @return: time in msec.
+ * NOTE that system tick is started when the NXT was turned on.
+ * not when application was started.
*/
U32 ecrobot_get_systick_ms(void)
{
return systick_get_ms();
}
-/*
- * Get NXT button state
+/**
+ * get ENTER buttons status
+ * NOTE that STOP and EXIT buttons are preserved by the system,
+ * so should not be disclosed to application.
+ *
+ * @return: 1(pressed)/0(not pressed)
+ */
+U8 ecrobot_is_ENTER_button_pressed(void)
+{
+ return ((ecrobot_get_button_state() & ENTER_PRESSED) == ENTER_PRESSED);
+}
+
+/**
+ * get RUN buttons status
+ * NOTE that STOP and EXIT buttons are preserved by the system,
+ * so should not be disclosed to application.
+ *
+ * @return: 1(pressed)/0(not pressed)
*/
+U8 ecrobot_is_RUN_button_pressed(void)
+{
+ return ((ecrobot_get_button_state() & RUN_PRESSED) == RUN_PRESSED);
+}
+
U8 ecrobot_get_button_state(void)
{
return ecrobot_inputs.buttons_state;
void ecrobot_poll_nxtstate(void)
{
static U8 buttons_states[N_BTN_STATE];
- static int buttons_i = 0;
- int i;
+ static SINT buttons_i = 0;
+ SINT i;
ecrobot_inputs.battery_state = battery_voltage();
}
}
-/*==============================================================================
- * ECRobot NXT Bluetooth API
- *=============================================================================*/
-/* Bluetooth data buffers */
-static U8 sendBuf[256];
-static U8 receiveBuf[256];
-
-/*
- * Send Bluetooth command
- * (dead copy of LEJOS Bluetooth.sendCommand)
- */
-static void ecrobot_send_bt_command(U8 *cmd, U32 len)
+void ecrobot_setDeviceInitialized(void)
{
- S32 checkSum = 0;
- S32 i;
-
- sendBuf[0] = (U8) (len + 2);
-
- for(i = 0; i < len; i++)
+ if (deviceStatus == DEVICE_NO_INIT)
{
- sendBuf[i+1] = cmd[i];
- checkSum += cmd[i];
+ deviceStatus = DEVICE_INITIALIZED;
}
+}
- checkSum = -checkSum;
- sendBuf[len+2] = (U8) ((checkSum >> 8) & 0xff);
- sendBuf[len+3] = (U8) (checkSum & 0xff);
- bt_send(sendBuf,len+3);
+/*==============================================================================
+ * NXT Data Logging API for NXT GamePad
+ *=============================================================================*/
+static S16 adc[4]; /* used for ecrobot_adc_data_monitor */
+/**
+ * data logging API used with NXT GamePad
+ *
+ * @param data1: user configurable data to be logged
+ * @param data2: user configurable data to be logged
+ */
+void ecrobot_bt_data_logger(S8 data1, S8 data2)
+{
+ U8 data_log_buffer[32];
+
+ *((U32 *)(&data_log_buffer[0])) = (U32)systick_get_ms();
+ *(( S8 *)(&data_log_buffer[4])) = (S8)data1;
+ *(( S8 *)(&data_log_buffer[5])) = (S8)data2;
+ *((U16 *)(&data_log_buffer[6])) = (U16)ecrobot_inputs.battery_state;
+ *((S32 *)(&data_log_buffer[8])) = (S32)nxt_motor_get_count(0);
+ *((S32 *)(&data_log_buffer[12])) = (S32)nxt_motor_get_count(1);
+ *((S32 *)(&data_log_buffer[16])) = (S32)nxt_motor_get_count(2);
+ *((S16 *)(&data_log_buffer[20])) = (S16)sensor_adc(0);
+ *((S16 *)(&data_log_buffer[22])) = (S16)sensor_adc(1);
+ *((S16 *)(&data_log_buffer[24])) = (S16)sensor_adc(2);
+ *((S16 *)(&data_log_buffer[26])) = (S16)sensor_adc(3);
+ *((S32 *)(&data_log_buffer[28])) = (S32)getDistance();
+
+ ecrobot_send_bt_packet(data_log_buffer, 32);
}
-/*
- * Receive Bluetooth reply or command
- * (dead copy of LEJOS Bluetooth.receiveReply)
+/**
+ * data logging API used with NXT GamePad
+ *
+ * @param data1: user configurable data to be logged
+ * @param data2: user configurable data to be logged
+ * @param adc1: user configurable data to be logged
+ * @param adc2: user configurable data to be logged
+ * @param adc3: user configurable data to be logged
+ * @param adc4: user configurable data to be logged
*/
-static S32 ecrobot_receive_bt_reply(U8 *buf, U32 bufLen)
+void ecrobot_bt_adc_data_logger(S8 data1, S8 data2, S16 adc1, S16 adc2, S16 adc3, S16 adc4)
{
- S32 checkSum, negSum, i;
- U32 len;
+ U8 data_log_buffer[32];
+
+ *((U32 *)(&data_log_buffer[0])) = (U32)systick_get_ms();
+ *(( S8 *)(&data_log_buffer[4])) = (S8)data1;
+ *(( S8 *)(&data_log_buffer[5])) = (S8)data2;
+ *((U16 *)(&data_log_buffer[6])) = (U16)ecrobot_inputs.battery_state;
+ *((S32 *)(&data_log_buffer[8])) = (S32)nxt_motor_get_count(0);
+ *((S32 *)(&data_log_buffer[12])) = (S32)nxt_motor_get_count(1);
+ *((S32 *)(&data_log_buffer[16])) = (S32)nxt_motor_get_count(2);
+ *((S16 *)(&data_log_buffer[20])) = (S16)adc1;
+ *((S16 *)(&data_log_buffer[22])) = (S16)adc2;
+ *((S16 *)(&data_log_buffer[24])) = (S16)adc3;
+ *((S16 *)(&data_log_buffer[26])) = (S16)adc4;
+ *((S32 *)(&data_log_buffer[28])) = (S32)getDistance();
+ adc[0] = adc1;
+ adc[1] = adc2;
+ adc[2] = adc3;
+ adc[3] = adc4;
+
+ ecrobot_send_bt_packet(data_log_buffer, 32);
+}
+
- memset(&receiveBuf[0],0,sizeof(receiveBuf));
- bt_receive(&receiveBuf[0]);
- len = receiveBuf[0];
- buf[0] = (U8) len;
+/*==============================================================================
+ * NXT LCD display API
+ *=============================================================================*/
- if (len == 0)
+/**
+ * convert a BMP file to an array data for LCD display
+ *
+ * @param file: monochrome BMP file data
+ * @param lcd: data array to be drawn in LCD
+ * @param width: pixel width of the BMP file (max. 100)
+ * @param height: pixel height of the BMP file (max. 64)
+ * @return: 1(success)/-1(failure)
+ */
+SINT ecrobot_bmp2lcd(const CHAR *file, U8 *lcd, S32 width, S32 height)
+{
+ SINT bmp_line, bmp_line_alignment;
+ SINT bmp_row, bmp_col;
+ SINT lcd_row;
+ SINT lcd_pos;
+ SINT bmp_bit_pos;
+ SINT lcd_bit_pos;
+ SINT bits;
+ U8 bmp_data;
+ BMP *bmp = (BMP *)file;
+
+ /* check a BMP file header information */
+ if (bmp->fileHeader.type != BM_TYPE) /* Windows */
+ return -1;
+ if ((bmp->infoHeader.width > NXT_LCD_WIDTH) ||
+ (bmp->infoHeader.width < 1) ||
+ (bmp->infoHeader.width != width))
+ return -1;
+ if ((bmp->infoHeader.height > NXT_LCD_DEPTH * 8) ||
+ (bmp->infoHeader.height < 1) ||
+ (bmp->infoHeader.height != height))
+ return -1;
+ if (bmp->infoHeader.bits != 1) /* monochrome bmp */
+ return -1;
+ if (bmp->infoHeader.compression != 0) /* no compression */
+ return -1;
+
+ /*
+ * Specifications of a monochrome bmp file
+ *
+ * - each bit represents a pixel.
+ * - each line has 4 bytes alignment.
+ * (100 pixels data is saved in 100/8 = 12 + 1 + 3 = 16 byte)
+ * - data order in a BMP file:
+ * (0,n)---------------->(m,n)
+ * ---------------->
+ * ---------------->
+ * ---------------->
+ * (0,0)---------------->(m,0)
+ */
+
+ /* calculate line alignment */
+ bmp_line = bmp->infoHeader.width / 8;
+ if (bmp->infoHeader.width % 8)
{
- return 0;
+ bmp_line++;
}
-
- checkSum = len;
-
- if (len-1 <= bufLen)
+
+ if ((bmp_line % 4) != 0)
{
- for(i = 1; i < len-1; i++)
- {
- buf[i] = receiveBuf[i];
- checkSum += (buf[i] & 0xff);
- }
- negSum = (receiveBuf[len-1] & 0xff) << 8;
- negSum += (receiveBuf[len] & 0xff);
- if (checkSum + negSum == 65536)
- {
- return len-1;
- }
- else
- {
- return 0;
- }
+ bmp_line_alignment = ((bmp_line / 4) + 1) * 4;
}
- return 0;
-}
-
-/*
- * Initialize Bluetooth connection
- */
-void ecrobot_init_bt_connection(void)
-{
- S32 i;
- U8 reply[32];
- U8 dummy[32];
- U8 msg[32];
- U8 device[7];
-
- if (gDevice_status == DEVICE_NO_INIT)
+ else
{
- /* low level initialization */
- bt_init();
-
- /* set Bluetooth command mode */
- bt_clear_arm7_cmd();
+ bmp_line_alignment = bmp_line;
}
- else if (gDevice_status == DEVICE_INITIALIZED)
+
+ /* convert BMP data to LCD array data */
+ for (bmp_row = 0; bmp_row < bmp->infoHeader.height; bmp_row++)
{
- /* searching for Bluetooth device
- * (based on LEJOS Bluetooth.waitForConnection)
- */
- ecrobot_receive_bt_reply(reply, 32);
-
- if (reply[0] != 0)
+ lcd_row = NXT_LCD_DEPTH - ((bmp_row / 8) + 1);
+ lcd_bit_pos = bmp_row % 8;
+ bits = 0;
+ for(bmp_col = 0; bmp_col < bmp_line; bmp_col++)
{
- if (reply[1] == MSG_REQUEST_PIN_CODE)
+ bmp_data = file[(bmp_row * bmp_line_alignment) + bmp_col + bmp->fileHeader.offset];
+ lcd_pos = (lcd_row * NXT_LCD_WIDTH) + (bmp_col * 8);
+ for (bmp_bit_pos = 0; ((bmp_bit_pos < 8) && (bits < bmp->infoHeader.width)); bmp_bit_pos++, bits++)
{
- for(i = 0; i < 7; i++)
- {
- device[i] = reply[i+2];
- }
- msg[0] = MSG_PIN_CODE;
- for(i = 0; i < 7; i++)
- {
- msg[i+1] = device[i];
- }
- /* PIN CODE is MATLAB */
- msg[8] = 'M';
- msg[9] = 'A';
- msg[10] = 'T';
- msg[11] = 'L';
- msg[12] = 'A';
- msg[13] = 'B';
- for(i = 0; i < 10; i++)
+ if (!(bmp_data & (0x01 << (7 - bmp_bit_pos))))
{
- msg[i+14] = 0;
+ lcd[lcd_pos + bmp_bit_pos] |= (0x80 >> lcd_bit_pos);
}
-
- ecrobot_send_bt_command(msg, 24);
- }
-
- if (reply[1] == MSG_REQUEST_CONNECTION)
- {
- for(i = 0; i < 7; i++)
+ else
{
- device[i] = reply[i+2];
+ lcd[lcd_pos + bmp_bit_pos] &= (~0x80 >> lcd_bit_pos);
}
- msg[0] = MSG_ACCEPT_CONNECTION;
- msg[1] = 1;
-
- ecrobot_send_bt_command(msg, 2);
- }
-
- /* Bluetooth connection was succedded */
- if (reply[1] == MSG_CONNECT_RESULT)
- {
- systick_wait_ms(300);
- ecrobot_receive_bt_reply(dummy,32);
-
- if (dummy[0] == 0)
- {
- msg[0] = MSG_OPEN_STREAM;
- msg[1] = reply[3];
-
- ecrobot_send_bt_command(msg, 2);
- systick_wait_ms(300);
-
- /* set Bluetooth stream mode */
- bt_set_arm7_cmd();
- gDevice_status = BLUETOOTH_CONNECTED;
- }
}
}
}
+ return 1;
}
-/*
- * Send Bluetooth data packet
- * (dead copy of LEJOS Bluetooth.sendPacket)
- */
-void ecrobot_send_bt_packet(U8 *buf, U32 bufLen)
-{
- S32 i;
-
- if (gDevice_status == BLUETOOTH_CONNECTED && bufLen <= 254)
- {
- sendBuf[0] = (U8) (bufLen & 0xFF);
- sendBuf[1] = (U8) ((bufLen >> 8) & 0xFF);
- for(i = 0; i < bufLen; i++)
- {
- sendBuf[i+2] = buf[i];
- }
- bt_send(&sendBuf[0],bufLen+2);
- }
-}
-
-/*
- * Read Bluetooth data packet
- * (dead copy of LEJOS Bluetooth.readPacket)
- */
-S32 ecrobot_read_bt_packet(U8 *buf, U32 bufLen)
-{
- S32 i;
- U32 len;
-
- if (gDevice_status == BLUETOOTH_CONNECTED)
- {
- bt_receive(&receiveBuf[0]);
- len = receiveBuf[0];
-
- if (len > 0 && len <= bufLen)
- {
- for(i = 0; i < len; i++)
- {
- buf[i] = receiveBuf[i+2];
- }
- return len;
- }
- return 0;
- }
- return 0;
-}
-
-/*
- * Terminate Bluetooth connection
- */
-void ecrobot_term_bt_connection(void)
-{
- /* set Bluetooth command mode */
- bt_clear_arm7_cmd();
-}
-
-/*==============================================================================
- * ECRobot NXT LCD display API
- *=============================================================================*/
void ecrobot_show_int(S32 var)
{
display_clear(0);
display_update();
}
-void ecrobot_debug(unsigned int var)
+void ecrobot_debug(UINT var)
{
display_clear(0);
display_update();
}
-void ecrobot_debug1(unsigned int var1, unsigned int var2, unsigned int var3)
+void ecrobot_debug1(UINT var1, UINT var2, UINT var3)
{
display_clear(0);
- display_goto_xy(0, 0);
- display_string((const char *)target_subsystem_name);
-
display_goto_xy(0, 1);
display_string("VAR1: ");
display_int(var1, 0);
display_update();
}
-void ecrobot_debug2(unsigned int var1, unsigned int var2, unsigned int var3)
+void ecrobot_debug2(UINT var1, UINT var2, UINT var3)
{
display_clear(0);
- display_goto_xy(0, 0);
- display_string((const char *)target_subsystem_name);
-
display_goto_xy(0, 4);
display_string("VAR4: ");
display_int(var1, 0);
display_update();
}
-void ecrobot_status_monitor(void)
+void ecrobot_status_monitor(const CHAR *target_name)
{
display_clear(0);
display_goto_xy(0, 0);
- display_string((const char *)target_subsystem_name);
+ display_string(target_name);
display_goto_xy(0, 1);
display_string("TIME:");
display_goto_xy(0, 7);
display_string("BT/DST: ");
- if (gDevice_status == BLUETOOTH_CONNECTED)
+ if (ecrobot_get_bt_status() == BT_STREAM)
+ {
+ display_unsigned(1, 0);
+ }
+ else
{
- display_int(1, 0);
+ display_unsigned(0, 0);
+ }
+ display_int(getDistance(), 5);
+
+ display_update();
+}
+
+/* This function is used to display user selected sensor values */
+void ecrobot_adc_data_monitor(const CHAR *target_name)
+{
+ display_clear(0);
+
+ display_goto_xy(0, 0);
+ display_string(target_name);
+
+ display_goto_xy(0, 1);
+ display_string("TIME:");
+ display_unsigned(systick_get_ms()/1000, 0);
+
+ display_goto_xy(0, 2);
+ display_string("BATT:");
+ display_unsigned(ecrobot_inputs.battery_state/100, 0);
+
+ display_goto_xy(0, 3);
+ display_string("REV: ");
+ display_int(nxt_motor_get_count(0), 0);
+ display_int(nxt_motor_get_count(1), 6);
+
+ display_goto_xy(0, 4);
+ display_string(" ");
+ display_int(nxt_motor_get_count(2), 0);
+
+ display_goto_xy(0, 5);
+ display_string("ADC1/2:");
+ display_int(adc[0], 0);
+ display_int(adc[1], 5);
+
+ display_goto_xy(0, 6);
+ display_string("ADC3/4:");
+ display_int(adc[2], 0);
+ display_int(adc[3], 5);
+
+ display_goto_xy(0, 7);
+ display_string("BT/DST: ");
+ if (ecrobot_get_bt_status() == BT_STREAM)
+ {
+ display_unsigned(1, 0);
}
else
{
- display_int(0, 0);
+ display_unsigned(0, 0);
}
- display_int(distance_state, 5);
+ display_int(getDistance(), 5);
display_update();
}
+
+/*==============================================================================
+ * NXT Sound API
+ *=============================================================================*/
+
+/**
+ * play a tone(wrapper of leJOS sound tone api)
+ *
+ * @param freq: soundfrequency in Hz (31 to 2100)
+ * @param ms: sound duration in centisecond (10 - 256)
+ * @vol: sound volume (0 to 100)
+ * @return 1 (always)
+ */
+SINT ecrobot_sound_tone(U32 freq, U32 ms, U32 vol)
+{
+ sound_freq_vol(freq, ms, vol);
+ return 1;
+}
+
+/**
+ * play a WAV file (support for only 8bit monoral PCM)
+ *
+ * @param file: WAV file data
+ * NOTE that supports only 8bit monoral PCM
+ * @param length: length of WAV file
+ * @param freq: sampling frequency -1(inherit the WAV file original)/2000 to 22050Hz
+ * @param vol: sound volume(0 to 100)
+ * @return: 1(success)/0(sound resource is busy)/-1(failure)
+ */
+SINT ecrobot_sound_wav(const CHAR *file, U32 length, S32 freq, U32 vol)
+{
+ WAV *wav = (WAV *)file;
+
+ /* check sound resource is free */
+ if (sound_get_time() > 0)
+ return 0;
+
+ /* check length of a file */
+ if (length < WAV_HDR_SIZE)
+ return -1;
+
+ /* check a WAV file header information */
+ if (wav->riff.chunkID != RIFF_CHUNK_ID)
+ return -1;
+ if (wav->riff.format != RIFF_FORMAT)
+ return -1;
+ if (wav->fmt.chunkID != FMT_CHUNK_ID)
+ return -1;
+ if (wav->fmt.audioFormat != 0x0001) /* PCM */
+ return -1;
+ if (wav->fmt.numChannels != 0x0001) /* mono channel */
+ return -1;
+ if (wav->fmt.bitsPerSample != 0x0008) /* 8bit */
+ return -1;
+
+ /* in case of freq < 0, freq(sample rate) is inherited from a WAV file */
+ if (freq < 0)
+ {
+ freq = wav->fmt.sampleRate;
+ }
+
+ /* read wav data. Currently, supported PCM file types are
+ * linear PCM(data chunkID is "data" and "fact") and non-linear PCM.
+ */
+ if (wav->data.chunkID == DATA_CHUNK_ID)
+ {
+ /* linear PCM */
+ sound_play_sample(wav->data.data, wav->data.chunkSize, (U32)freq, vol);
+ }
+ else
+ {
+ WAV_FACT *wav_fact = (WAV_FACT *)file;
+ if (wav_fact->data.chunkID == FACT_CHUNK_ID && wav_fact->data.data_chunkID == DATA_CHUNK_ID)
+ {
+ /* linear PCM with "fact" chunk ID */
+ sound_play_sample(wav_fact->data.data, wav_fact->data.data_chunkSize, (U32)freq, vol);
+ }
+ else
+ {
+ WAV_NL *wav_nl = (WAV_NL *)file;
+ if (wav_nl->data.chunkID == DATA_CHUNK_ID)
+ {
+ /* non linear PCM */
+ sound_play_sample(wav_nl->data.data, wav_nl->data.chunkSize, (U32)freq, vol);
+ }
+ else
+ {
+ return -1;
+ }
+ }
+ }
+ return 1;
+}
+
+
+/*==============================================================================
+ * NXT USB API
+ *=============================================================================*/
+static U8 usb_reset_flag = 0;
+
+/**
+ * reset USB.
+ * This API can be invoked in either ecrobot_device_initialize
+ * and run-time application
+ */
+void ecrobot_init_usb(void)
+{
+ /* invoked in ecrobot_device_initialize */
+ if(!get_OS_flag())
+ {
+ /* invoked only for the first time */
+ if (deviceStatus == DEVICE_NO_INIT)
+ {
+ udp_reset();
+ usb_reset_flag = 1;
+ add_status_info(USB_INITIALIZED);
+ }
+ }
+ /* invoked in run-time application */
+ else
+ {
+ udp_reset();
+ usb_reset_flag = 1;
+ }
+}
+
+/**
+ * read USB data from host
+ *
+ * @param buf: buffer to return data
+ * @param len: length of the return data
+ * @return: 1(success)/0(failure)
+ */
+SINT ecrobot_read_usb(U8 *buf, U32 len)
+{
+ if (usb_reset_flag != 1)
+ return 0;
+
+ if (len > MAX_USB_DATA_SIZE)
+ return 0;
+
+ return udp_read(buf, len);
+}
+
+/**
+ * send USB data to host
+ *
+ * @param buf: buffer containing data to send
+ * @param len: length of the data to send
+ * @return: 1(success)/0(failure)
+ */
+SINT ecrobot_send_usb(U8 *buf, U32 len)
+{
+ if (usb_reset_flag != 1)
+ return 0;
+
+ if (len > MAX_USB_DATA_SIZE)
+ return 0;
+
+ udp_write(buf, len);
+ return len;
+}
+
+/**
+ * close a USB connection. (currently it's just reset the flag)
+ */
+void ecrobot_term_usb(void)
+{
+ usb_reset_flag = 0;
+ udp_close(0);
+}
OSDN Git Service
