diff --git a/Makefile b/Makefile
index 7ae35dfb01c461273304bd41ba16138e6305d6f7..3bc66eeedf31d86bf0d88eefe3bd367a749967c9 100755
--- a/Makefile
+++ b/Makefile
@@ -3,14 +3,14 @@
PROJECT_NAME=darwin_firmware
TARGET_FILES=$(wildcard src/*.c)
-TARGET_PROCESSOR=STM32F103C8
+TARGET_PROCESSOR=STM32F103RE
include ../../STM32_processor/libraries/f1/select_processor.mk
STM32_LIB_PATH = ../../STM32_processor/libraries/f1/stm32_lib
DYNAMIXEL_LIB_PATH = ../../STM32_processor/libraries/f1/dynamixel
STM32_STARTUP_FILES_PATH = ../../STM32_processor/startup/f1
-STM32_LINKER_SCRIPTS_PATH = ../../STM32_processor/libraries/f1/linker_scripts
+STM32_LINKER_SCRIPTS_PATH = ./linker_script
BUILD_PATH=build
COMPILE_OPTS = -mlittle-endian -mcpu=cortex-m3 -mthumb -mthumb-interwork
@@ -28,7 +28,7 @@ AS = $(TCHAIN_PREFIX)gcc
ASFLAGS = $(COMPILE_OPTS) -c
LD = $(TCHAIN_PREFIX)gcc
-LDFLAGS = -mthumb -mcpu=cortex-m3 -Wl,-Map=$@.map,-cref $(INCLUDE_DIRS) $(LIBRARY_DIRS) -T $(STM32_LINKER_SCRIPTS_PATH)/$(LINKER_SCRIPT_FILE) -nostartfiles
+LDFLAGS = -mthumb -mcpu=cortex-m3 -Wl,-Map=$@.map,-cref $(INCLUDE_DIRS) $(LIBRARY_DIRS) -T $(STM32_LINKER_SCRIPTS_PATH)/darwin.ld -nostartfiles
OBJCP = $(TCHAIN_PREFIX)objcopy
OBJCPFLAGS_HEX = -O ihex
diff --git a/include/action.h b/include/action.h
index 8c063bfb7f29aeee38bd4580af1dd2f88c2e9114..29e4eff35f432037a2857d8a118833f3e1420acb 100755
--- a/include/action.h
+++ b/include/action.h
@@ -2,10 +2,15 @@
#define _ACTION_H
#include "stm32f10x.h"
+#include "motion_manager.h"
-// basic motion interface
-void action_init(void);
-uint8_t action_set_page(uint8_t page_id);
+extern int64_t action_current_angles[MANAGER_MAX_NUM_SERVOS];
+
+// public functions
+void action_init(uint16_t period);
+inline void action_set_period(uint16_t period);
+inline uint16_t action_get_period(void);
+uint8_t action_load_page(uint8_t page_id);
void action_start_page(void);
void action_stop_page(void);
uint8_t action_is_running(void);
diff --git a/include/adc.h b/include/adc.h
deleted file mode 100755
index 4f950f9ed5b160cadd4ed099143b135bd9623e82..0000000000000000000000000000000000000000
--- a/include/adc.h
+++ /dev/null
@@ -1,13 +0,0 @@
-#ifndef _ADC_H
-#define _ADC_H
-
-#include "stm32f10x.h"
-
-#define NUM_ADC_CHANNELS 16
-
-// public functions
-void adc_init(void);
-uint16_t adc_get_channel(uint8_t channel);
-uint16_t adc_get_averaged_channel(uint8_t channel);
-
-#endif
diff --git a/include/adc_dma.h b/include/adc_dma.h
new file mode 100755
index 0000000000000000000000000000000000000000..20308ccc2b4b25ec35a1a88a47c1bf546b57b117
--- /dev/null
+++ b/include/adc_dma.h
@@ -0,0 +1,8 @@
+#ifndef _ADC_DMA_H
+#define _ADC_DMA_H
+
+#include "stm32f10x.h"
+
+void adc_init(void);
+
+#endif
diff --git a/include/darwin_kinematics.h b/include/darwin_kinematics.h
new file mode 100755
index 0000000000000000000000000000000000000000..40c2dabe808bff6ce3ddb4b82f591f49488d8b7d
--- /dev/null
+++ b/include/darwin_kinematics.h
@@ -0,0 +1,15 @@
+#ifndef _DARWIN_KINEMATICS_H
+#define _DARWIN_KINEMATICS_H
+
+#include "stm32f10x.h"
+
+extern const float LEG_SIDE_OFFSET; //mm
+extern const float THIGH_LENGTH; //mm
+extern const float CALF_LENGTH; //mm
+extern const float ANKLE_LENGTH; //mm
+extern const float LEG_LENGTH; //mm (THIGH_LENGTH + CALF_LENGTH + ANKLE_LENGTH)
+
+// public functions
+uint8_t computeIK(float *out, float x, float y, float z, float a, float b, float c);
+
+#endif
diff --git a/include/darwin_math.h b/include/darwin_math.h
new file mode 100755
index 0000000000000000000000000000000000000000..6fd0f81afa2ad7ff0a408fcb4b8c2d41537c965d
--- /dev/null
+++ b/include/darwin_math.h
@@ -0,0 +1,63 @@
+#ifndef _DARWIN_MATH_H
+#define _DARWIN_MATH_H
+
+#include "stm32f10x.h"
+#include <math.h>
+
+#define PI 3.14159
+
+typedef struct{
+ float x;
+ float y;
+ float z;
+}TPoint3D;
+
+// point public functions
+void point3d_init(TPoint3D *point);
+void point3d_load(TPoint3D *point, float x, float y, float z);
+void point3d_copy(TPoint3D *point_dst, TPoint3D *point_src);
+
+typedef struct {
+ float x;
+ float y;
+ float z;
+}TVector3D;
+
+// vector public functions
+void vector3d_init(TVector3D *vector);
+void vector3d_load(TVector3D *vector, float x, float y, float z);
+void vector3d_copy(TVector3D *vector_dst, TVector3D *vector_src);
+float vector3d_length(TVector3D *vector);
+
+enum{
+ m00=0,
+ m01,
+ m02,
+ m03,
+ m10,
+ m11,
+ m12,
+ m13,
+ m20,
+ m21,
+ m22,
+ m23,
+ m30,
+ m31,
+ m32,
+ m33,
+};
+
+typedef struct{
+ float coef[16];
+}TMatrix3D;
+
+// matrix public functions
+void matrix3d_init(TMatrix3D *matrix);
+void matrix3d_copy(TMatrix3D *matrix_dst, TMatrix3D *matrix_src);
+void matrix3d_identity(TMatrix3D *matrix);
+uint8_t matrix3d_inverse(TMatrix3D *org, TMatrix3D *inv);
+void matrix3d_set_transform(TMatrix3D *matrix, TPoint3D *point, TVector3D *vector);
+void matrix3d_mult(TMatrix3D *dst,TMatrix3D *a,TMatrix3D *b);
+
+#endif
diff --git a/include/dyn_servos.h b/include/dyn_servos.h
index 05e2eb8692ac06384d7e4f80f7aed38eb7d6a8b1..775f26d6ccbede393c8efa80a9da52f2c6c558e9 100755
--- a/include/dyn_servos.h
+++ b/include/dyn_servos.h
@@ -2,14 +2,16 @@
#define _DYN_SERVOS_H
// servo models
-#define SERVO_AX12 0x000C
-#define SERVO_AX18 0x0012
-#define SERVO_RX10 0x000A
+#define SERVO_AX12A 0x000C
+#define SERVO_AX12W 0x012C
+#define SERVO_AX18A 0x0012
+#define SERVO_MX28 0x001D
#define SERVO_RX24F 0x0018
#define SERVO_RX28 0x001C
#define SERVO_RX64 0x0040
+#define SERVO_MX64 0x0136
#define SERVO_EX106 0x006B
-#define SERVO_MX28 0x001D
+#define SERVO_MX106 0x0140
// Servo register map
typedef enum{
diff --git a/include/dynamixel_master.h b/include/dynamixel_master_uart_dma.h
similarity index 81%
rename from include/dynamixel_master.h
rename to include/dynamixel_master_uart_dma.h
index 2ac00eb4fe02e89835d9369ca9ec0a8562a8036f..1a2916a3057bbbce52ced79291e9c644ee969a7e 100755
--- a/include/dynamixel_master.h
+++ b/include/dynamixel_master_uart_dma.h
@@ -1,20 +1,17 @@
-#ifndef _DYNAMIXEL_MASTER_H
-#define _DYNAMIXEL_MASTER_H
+#ifndef _DYNAMIXEL_MASTER_DMA_H
+#define _DYNAMIXEL_MASTER_DMA_H
-#include "dynamixel.h"
#include "stm32f10x.h"
-
-// interrupt handlers
-void USART1_IRQHandler(void);
+#include "dynamixel.h"
// dynamixel master functions
void dyn_master_init(void);
void dyn_master_flush(void);
-void dyn_master_power_on(void);
-void dyn_master_power_off(void);
-uint8_t dyn_master_is_powered_on(void);
void dyn_master_set_timeout(uint16_t timeout_ms);
void dyn_master_scan(uint8_t *num,uint8_t *ids);
+inline void dyn_master_enable_power(void);
+inline void dyn_master_disable_power(void);
+uint8_t dyn_master_is_power_enabled(void);
uint8_t dyn_master_ping(uint8_t id);
uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data);
uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data);
@@ -25,7 +22,6 @@ uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint
uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data);
void dyn_master_action(void);
void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, uint8_t *data);
-uint8_t dyn_master_reset(uint8_t id);
// repeater functions
uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet);
diff --git a/include/dynamixel_slave.h b/include/dynamixel_slave.h
deleted file mode 100755
index b8b90cd416fa7a1230365dbd298514023c89620c..0000000000000000000000000000000000000000
--- a/include/dynamixel_slave.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef _DYNAMIXEL_SLAVE_H
-#define _DYNAXIXEL_SLAVE_H
-
-#include "dynamixel.h"
-#include "stm32f10x.h"
-
-// interrupt handlers
-void USART3_IRQHandler(void);
-
-// public functions
-void dyn_slave_init(void);
-uint8_t dyn_slave_pc_present(void);
-void dyn_slave_set_address(uint8_t id);
-uint8_t dyn_slave_get_address(void);
-uint8_t dyn_slave_is_packet_ready(void);
-void dyn_slave_get_inst_packet(uint8_t *packet);
-void dyn_slave_send_status_packet(uint8_t error,uint8_t length, uint8_t *data);
-void dyn_slave_resend_status_packet(uint8_t *packet);
-
-#endif
diff --git a/include/dynamixel_slave_uart_dma.h b/include/dynamixel_slave_uart_dma.h
new file mode 100755
index 0000000000000000000000000000000000000000..88b0a66f62b065cb3d223c444eece4df1afcefd7
--- /dev/null
+++ b/include/dynamixel_slave_uart_dma.h
@@ -0,0 +1,20 @@
+#ifndef _DYNAMIXEL_SLAVE_UART_DMA_H
+#define _DYNAXIXEL_SLAVE_UART_DMA_H
+
+#include "dynamixel.h"
+#include "stm32f10x.h"
+
+// public functions
+void dyn_slave_init(void);
+inline void dyn_slave_set_address(uint8_t id);
+inline uint8_t dyn_slave_get_address(void);
+inline void dyn_slave_set_return_delay(uint8_t delay);
+inline void dyn_slave_set_return_level(uint8_t level);
+inline uint8_t dyn_slave_get_return_level(void);
+uint8_t dyn_slave_is_packet_ready(void);
+void dyn_slave_get_inst_packet(uint8_t *packet);
+void dyn_slave_send_status_packet(uint8_t error,uint8_t length, uint8_t *data);
+void dyn_slave_resend_status_packet(uint8_t *packet);
+void dyn_slave_reset(void);
+
+#endif
diff --git a/include/eeprom.h b/include/eeprom.h
index 32b42d8f1ea28fe101e6bc37540396139b883b48..33c9ce9f2c6ee9cdbd4ee831d913b6351f9ff9cb 100755
--- a/include/eeprom.h
+++ b/include/eeprom.h
@@ -1,22 +1,89 @@
-#ifndef _EEPROM_H
-#define _EEPROM_H
-
-#include "stm32f10x.h"
-
-#define EEPROM_DATA_SIZE 0x18
-
-// EEPROM errors
-#define EEPROM_SUCCESS 0
-#define EEPROM_BAD_ADDRESS -1
-
-// public functions
-uint16_t eeprom_read_device_model(void);
-uint8_t eeprom_read_firmware_version(void);
-uint8_t eeprom_read_device_id(void);
-uint8_t eeprom_read_baudrate(void);
-uint8_t eeprom_read_return_delay(void);
-uint8_t eeprom_read_return_level(void);
-uint8_t eeprom_read_table(uint32_t start_offset, int16_t length,uint8_t *data);
-uint8_t eeprom_write_table(uint32_t start_offset, int16_t length,uint8_t *data);
-
-#endif
+/**
+ ******************************************************************************
+ * @file EEPROM_Emulation/inc/eeprom.h
+ * @author MCD Application Team
+ * @version V1.0.0
+ * @date 10-October-2011
+ * @brief This file contains all the functions prototypes for the EEPROM
+ * emulation firmware library.
+ ******************************************************************************
+ * @attention
+ *
+ * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+ * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
+ * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
+ * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
+ * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
+ * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+ *
+ * <h2><center>© COPYRIGHT 2011 STMicroelectronics</center></h2>
+ ******************************************************************************
+ */
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __EEPROM_H
+#define __EEPROM_H
+
+/* Includes ------------------------------------------------------------------*/
+#include "stm32f10x.h"
+
+/* Exported constants --------------------------------------------------------*/
+/* Define the size of the sectors to be used */
+#define PAGE_SIZE (uint32_t)0x0800 /* Page size = 2KByte */
+
+/* Device voltage range supposed to be [2.7V to 3.6V], the operation will
+ be done by word */
+#define VOLTAGE_RANGE (uint8_t)VoltageRange_3
+
+/* EEPROM start address in Flash */
+#define EEPROM_START_ADDRESS ((uint32_t)0x08003800) /* EEPROM emulation start address:
+ from sector2 : after 16KByte of used
+ Flash memory */
+
+/* Pages 0 and 1 base and end addresses */
+#define PAGE0_BASE_ADDRESS ((uint32_t)(EEPROM_START_ADDRESS + 0x0000))
+#define PAGE0_END_ADDRESS ((uint32_t)(EEPROM_START_ADDRESS + (PAGE_SIZE - 1)))
+
+#define PAGE1_BASE_ADDRESS ((uint32_t)(EEPROM_START_ADDRESS + 0x0800))
+#define PAGE1_END_ADDRESS ((uint32_t)(EEPROM_START_ADDRESS + (2 * PAGE_SIZE - 1)))
+
+/* Used Flash pages for EEPROM emulation */
+#define PAGE0 ((uint16_t)0x0000)
+#define PAGE1 ((uint16_t)0x0001)
+
+/* No valid page define */
+#define NO_VALID_PAGE ((uint16_t)0x00AB)
+
+/* Page status definitions */
+#define ERASED ((uint16_t)0xFFFF) /* Page is empty */
+#define RECEIVE_DATA ((uint16_t)0xEEEE) /* Page is marked to receive data */
+#define VALID_PAGE ((uint16_t)0x0000) /* Page containing valid data */
+
+/* Valid pages in read and write defines */
+#define READ_FROM_VALID_PAGE ((uint8_t)0x00)
+#define WRITE_IN_VALID_PAGE ((uint8_t)0x01)
+
+/* Page full define */
+#define PAGE_FULL ((uint8_t)0x80)
+
+/* Virtual address defined by the user: 0xFFFF value is prohibited */
+#define DEVICE_MODEL_OFFSET ((uint16_t)0x0000)
+#define FIRMWARE_VERSION_OFFSET ((uint16_t)0x0002)
+#define DEVICE_ID_OFFSET ((uint16_t)0x0003)
+#define BAUDRATE_OFFSET ((uint16_t)0x0004)
+#define RETURN_DELAY_OFFSET ((uint16_t)0x0005)
+#define MM_PERIOD_OFFSET ((uint16_t)0x0006)
+#define RETURN_LEVEL_OFFSET ((uint16_t)0x0010)
+
+#define LAST_EEPROM_OFFSET ((uint16_t)0x0017)
+
+/* Exported types ------------------------------------------------------------*/
+/* Exported macro ------------------------------------------------------------*/
+/* Exported functions ------------------------------------------------------- */
+uint16_t EE_Init(void);
+uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data);
+uint16_t EE_WriteVariable(uint16_t VirtAddress, uint16_t Data);
+
+#endif /* __EEPROM_H */
+
+/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
diff --git a/include/gpio.h b/include/gpio.h
index b8035bd0743c6086beab5c8ab5b2b619ca9d6070..755d91add31463bb3b7efef0bf24b1ecca075dd5 100755
--- a/include/gpio.h
+++ b/include/gpio.h
@@ -3,52 +3,18 @@
#include "stm32f10x.h"
-// interrupt handler
-void TIM2_IRQHandler(void);
+typedef enum {LED_TX,LED_RX,LED_2,LED_3,LED_4,LED_5_R,LED_5_G,LED_5_B,LED_6_R,LED_6_G,LED_6_B} led_t;
-// configuration functions
-void gpio_init(void);
-
-// LED 2 functions
-void set_led2(void);
-void clear_led2(void);
-void toggle_led2(void);
-void blink_led2(uint16_t period_ms);
-
-// LED 3 functions
-void set_led3(void);
-void clear_led3(void);
-void toggle_led3(void);
-void blink_led4(uint16_t period_ms);
-
-// LED 4 functions
-void set_led4(void);
-void clear_led4(void);
-void toggle_led4(void);
-void blink_led4(uint16_t period_ms);
+typedef enum {START_PB,MODE_PB} pushbutton_t;
-// RX LED functions
-void set_rx_led(void);
-void clear_rx_led(void);
-void toggle_rx_led(void);
-void blink_rx_led(uint16_t period_ms);
-
-// TX LED functions
-void set_tx_led(void);
-void clear_tx_led(void);
-void toggle_tx_led(void);
-void blink_tx_led(uint16_t period_ms);
-
-// RGB LED's
-void set_eyes_color(uint8_t r,uint8_t g, uint8_t b);
-void get_eyes_color(uint8_t *r,uint8_t *g, uint8_t *b);
-void set_head_color(uint8_t r,uint8_t g, uint8_t b);
-void get_head_color(uint8_t *r,uint8_t *g, uint8_t *b);
-
-// pushbuttons
-uint8_t is_mode_pushed(void);
-void mode_attach_function(void (*sw_function)(void));
-uint8_t is_start_pushed(void);
-void start_attach_function(void (*sw_function)(void));
+void gpio_init(void);
+// LED functions
+void gpio_set_led(led_t led_id);
+void gpio_clear_led(led_t led_id);
+void gpio_toggle_led(led_t led_id);
+void gpio_blink_led(led_t led_id, int16_t period_ms);
+// Pushbuttons functions
+uint8_t gpio_is_pushbutton_pressed(pushbutton_t pb_id);
+void gpio_set_pushbutton_callback(pushbutton_t pb_id,void (*callback)(void));
#endif
diff --git a/include/imu.h b/include/imu.h
index e617e67ca1031b6e52d2dba0748b371c7716d81b..ef91bc9a17d61977a7815d79e9e19a64893f876b 100755
--- a/include/imu.h
+++ b/include/imu.h
@@ -5,7 +5,8 @@
//public functions
void imu_init(void);
-void imu_get_gyroscope(int16_t *x, int16_t *y, int16_t *z);
-void imu_get_accelerometer(int16_t *x, int16_t *y, int16_t *z);
+void imu_start(void);
+void imu_stop(void);
+void imu_accel_get_data(void);
#endif
diff --git a/include/joint_motion.h b/include/joint_motion.h
deleted file mode 100755
index 1c64274050a5f43b8c602e4fc4819a61c7358d49..0000000000000000000000000000000000000000
--- a/include/joint_motion.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#ifndef _JOINT_MOTION_H
-#define _JOINT_MOTION_H
-
-#include "stm32f10x.h"
-
-#define JOINT_MAX_NUM_SERVOS 30
-
-// public functions
-void joint_motion_init(void);
-void joint_motion_load_angles(uint8_t first,uint8_t num_servos, int16_t *angles);
-void joint_motion_load_speeds(uint8_t first,uint8_t num_servos, uint8_t *speeds);
-void joint_motion_load_accels(uint8_t first,uint8_t num_servos, uint8_t *accels);
-void joint_motion_start(void);
-void joint_motion_stop(void);
-void joint_motion_pause(void);
-uint8_t joint_motion_is_running(void);
-
-// motion manager interface
-void joint_motion_process(void);
-
-#endif
diff --git a/include/matrix.h b/include/matrix.h
deleted file mode 100755
index 6b39f0547a7c1cab0fc2b61ee09f561554d9ba26..0000000000000000000000000000000000000000
--- a/include/matrix.h
+++ /dev/null
@@ -1,40 +0,0 @@
-#ifndef _MATRIX_H_
-#define _MATRIX_H_
-
-#include "stm32f10x.h"
-
-#include "vector.h"
-#include "point.h"
-
-enum{
- m00=0,
- m01,
- m02,
- m03,
- m10,
- m11,
- m12,
- m13,
- m20,
- m21,
- m22,
- m23,
- m30,
- m31,
- m32,
- m33,
-};
-
-typedef struct{
- float coef[16];
-}TMatrix3D;
-
-// public functions
-void matrix3d_init(TMatrix3D *matrix);
-void matrix3d_copy(TMatrix3D *matrix_dst, TMatrix3D *matrix_src);
-void matrix3d_identity(TMatrix3D *matrix);
-uint8_t matrix3d_inverse(TMatrix3D *org, TMatrix3D *inv);
-void matrix3d_set_transform(TMatrix3D *matrix, TPoint3D *point, TVector3D *vector);
-void matrix3d_mult(TMatrix3D *dst,TMatrix3D *a,TMatrix3D *b);
-
-#endif
diff --git a/include/motion_manager.h b/include/motion_manager.h
index a1c4878c345a12b86bb0bcadd384789a6a13d95e..050812e51c95429eaba92c2a653af63cc7f5ecc9 100755
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -3,6 +3,13 @@
#include "stm32f10x.h"
+#define MANAGER_MAX_NUM_SERVOS 31
+
+typedef enum{MM_NONE = 0,
+ MM_ACTION = 1,
+ MM_WALKING = 2,
+ MM_JOINTS = 3} TModules;
+
// default joint identifiers
enum{
R_SHOULDER_PITCH = 1,
@@ -27,51 +34,34 @@ enum{
HEAD_TILT = 20
};
-// motion modules
-typedef enum{MM_NONE = 0,
- MM_ACTION = 1,
- MM_WALKING = 2,
- MM_JOINTS = 3}TModules;
-
-#define MAX_NUM_SERVOS 254
-
// servo information structure
typedef struct{
uint8_t id;
- uint8_t model;
- uint16_t max_value;
- uint16_t min_value;
- uint16_t center_value;
- uint16_t current_value;
- int16_t max_angle;
- int16_t min_angle;
+ uint16_t model;
+ uint16_t encoder_resolution;
+ uint8_t gear_ratio;
+ uint16_t max_angle;
+ uint16_t center_angle;
+ uint16_t max_speed;
int16_t current_angle;
- uint8_t module;
+ uint16_t current_value;
+ TModules module;
+ uint8_t enabled;
}TServoInfo;
-extern const int8_t dir[14];
-
-// interrupt handler
-void TIM3_IRQHandler(void);
-
// public functions
-void manager_init(void);
-void manager_enable(void);
-void manager_disable(void);
-uint8_t manager_is_enabled(void);
+void manager_init(uint16_t period_us);
+inline uint16_t manager_get_period(void);
+inline void manager_set_period(uint16_t period_us);
+inline void manager_enable(void);
+inline void manager_disable(void);
+inline uint8_t manager_is_enabled(void);
inline uint8_t manager_get_num_servos(void);
-inline uint8_t manager_get_period(void);
-void manager_read_current_position(void);
-void manager_set_servo_angle(uint8_t servo_id, int16_t angle);
-void manager_set_servo_value(uint8_t servo_id, uint16_t value);
-void manager_set_module(uint8_t servo_id,TModules module);
-void manager_set_index_angle(uint8_t servo_id, int16_t angle);
-void manager_set_index_value(uint8_t servo_id, uint16_t value);
-inline int16_t manager_get_servo_angle(uint8_t servo_id);
-inline uint16_t manager_get_servo_value(uint8_t servo_id);
-inline TModules manager_get_servo_module(uint8_t servo_id);
-inline int16_t manager_get_index_angle(uint8_t servo_id);
-inline uint16_t manager_get_index_value(uint8_t servo_id);
-inline TModules manager_get_index_module(uint8_t servo_id);
+inline void manager_set_module(uint8_t servo_id,TModules module);
+inline TModules manager_get_module(uint8_t servo_id);
+inline void manager_enable_servo(uint8_t servo_id);
+inline void manager_disable_servo(uint8_t servo_id);
+void manager_disable_servos(void);
+inline uint8_t manager_is_servo_enabled(uint8_t servo_id);
#endif
diff --git a/include/motion_pages.h b/include/motion_pages.h
index b26ff5040799fe01fa61f7a1551a66d6af946922..5d3f3fea6f051266bda506fceefd68c115963d88 100755
--- a/include/motion_pages.h
+++ b/include/motion_pages.h
@@ -3,17 +3,13 @@
#include "stm32f10x.h"
-#define PAGE_SUCCESS 0
-#define PAGE_BAD_CHECKSUM -1
-#define PAGE_INVALID -2
-#define PAGE_INVALID_POSE -3
-
-#define PAGE_MAX_NUM_SERVOS 31
-#define POSE_NUMBER_OF_POSES_PER_PAGE 7
+#define MAX_PAGES 256
+#define PAGE_MAX_NUM_SERVOS 31
+#define POSE_NUMBER_OF_POSES_PER_PAGE 7
typedef struct // Header Structure (total 64unsigned char)
{
- uint8_t name[13]; // Name 0~13
+ uint8_t name[14]; // Name 0~13
uint8_t reserved1; // Reserved1 14
uint8_t repeat; // Repeat count 15
uint8_t schedule; // schedule 16
@@ -33,7 +29,7 @@ typedef struct // Header Structure (total 64unsigned char)
typedef struct // Step Structure (total 64unsigned char)
{
- uint16_t position[PAGE_MAX_NUM_SERVOS]; // Joint position 0~61
+ int16_t position[PAGE_MAX_NUM_SERVOS]; // Joint position 0~61
uint8_t pause; // Pause time 62
uint8_t time; // Time 63
} TStep;
@@ -44,7 +40,12 @@ typedef struct // Page Structure (total 512unsigned char)
TStep steps[POSE_NUMBER_OF_POSES_PER_PAGE]; // Page step 65~511
} TPage;
-uint8_t load_page_info(uint8_t page_id,TPage *page);
-uint8_t get_offsets(uint16_t *offsets);
+extern TPage motion_pages[46];
+
+// public functions
+void pages_get_page(uint8_t page_id,TPage *page);
+uint8_t pages_check_checksum(TPage *page);
+void pages_clear_page(TPage *page);
+void pages_copy_page(TPage *src,TPage *dst);
#endif
diff --git a/include/point.h b/include/point.h
deleted file mode 100755
index e4452c0c24937d42eb1b773cd89f3ebf0ace5da4..0000000000000000000000000000000000000000
--- a/include/point.h
+++ /dev/null
@@ -1,15 +0,0 @@
-#ifndef _POINT_H_
-#define _POINT_H_
-
-typedef struct{
- float x;
- float y;
- float z;
-}TPoint3D;
-
-// public functions
-void point3d_init(TPoint3D *point);
-void point3d_load(TPoint3D *point, float x, float y, float z);
-void point3d_copy(TPoint3D *point_dst, TPoint3D *point_src);
-
-#endif
diff --git a/include/ram.h b/include/ram.h
index 8683fbd3fefc536c28a68c74f5c5fc66ee78e364..335bdaed2b87735e3936547351030b8cf3e1eef8 100755
--- a/include/ram.h
+++ b/include/ram.h
@@ -2,257 +2,194 @@
#define _RAM_H
#include "stm32f10x.h"
+#include "eeprom.h"
-#define RAM_SIZE 256
-
-#define RAM_SUCCESS 0
+#define RAM_SUCCESS 0
#define RAM_BAD_ADDRESS -1
#define RAM_WRITE_ERROR -2
#define RAM_BAD_BIT -3
+#define RAM_BAD_ACCESS -4
-// CM730 register map
typedef enum {
-/* EEPROM AREA */
- CM730_MODEL_NUMBER_L = 0,
- CM730_MODEL_NUMBER_H = 1,
- CM730_VERSION = 2,
- CM730_ID = 3,
- CM730_BAUD_RATE = 4,
- CM730_RETURN_DELAY_TIME = 5,
- CM730_RETURN_LEVEL = 16,
-/* RAM AREA */
- CM730_DXL_POWER = 24,
- CM730_LED_PANNEL = 25,
- CM730_LED_HEAD_L = 26,
- CM730_LED_HEAD_H = 27,
- CM730_LED_EYE_L = 28,
- CM730_LED_EYE_H = 29,
- CM730_BUTTON = 30,
-/* Action control registers*/
- CM730_ACTION_NUMBER = 31,
- CM730_ACTION_CONTROL = 32,
- CM730_ACTION_STATUS = 33,
-/* Walking control interface */
- CM730_WALKING_CONTROL = 34,
- CM730_WALKING_STATUS = 35,
-/* joint motion control interface */
- CM730_JOINT_CONTROL = 36,
- CM730_JOINT_STATUS = 37,
-/* Inertial sensors */
- CM730_GYRO_Z_L = 38,
- CM730_GYRO_Z_H = 39,
- CM730_GYRO_Y_L = 40,
- CM730_GYRO_Y_H = 41,
- CM730_GYRO_X_L = 42,
- CM730_GYRO_X_H = 43,
- CM730_ACCEL_X_L = 44,
- CM730_ACCEL_X_H = 45,
- CM730_ACCEL_Y_L = 46,
- CM730_ACCEL_Y_H = 47,
- CM730_ACCEL_Z_L = 48,
- CM730_ACCEL_Z_H = 49,
-/* Analof to digital interface */
- CM730_VOLTAGE = 50,
- CM730_LEFT_MIC_L = 51,
- CM730_LEFT_MIC_H = 52,
- CM730_ADC2_L = 53,
- CM730_ADC2_H = 54,
- CM730_ADC3_L = 55,
- CM730_ADC3_H = 56,
- CM730_ADC4_L = 57,
- CM730_ADC4_H = 58,
- CM730_ADC5_L = 59,
- CM730_ADC5_H = 60,
- CM730_ADC6_L = 61,
- CM730_ADC6_H = 62,
- CM730_ADC7_L = 63,
- CM730_ADC7_H = 64,
- CM730_ADC8_L = 65,
- CM730_ADC8_H = 66,
- CM730_RIGHT_MIC_L = 67,
- CM730_RIGHT_MIC_H = 68,
- CM730_ADC10_L = 69,
- CM730_ADC10_H = 70,
- CM730_ADC11_L = 71,
- CM730_ADC11_H = 72,
- CM730_ADC12_L = 73,
- CM730_ADC12_H = 74,
- CM730_ADC13_L = 75,
- CM730_ADC13_H = 76,
- CM730_ADC14_L = 77,
- CM730_ADC14_H = 78,
- CM730_ADC15_L = 79,
- CM730_ADC15_H = 80,
- CM730_BUZZER_DATA0 = 81,
- CM730_BUZZER_DATA1 = 82,
- CM730_TX_REMOCON_DATA_L = 83,
- CM730_TX_REMOCON_DATA_H = 84,
- CM730_RX_REMOCON_DATA_L = 85,
- CM730_RX_REMOCON_DATA_H = 86,
- CM730_RX_REMOCON_DATA_ARRIVED = 87,
- CM730_ZIGBEE_ID_L = 88,
- CM730_ZIGBEE_ID_H = 89,
-/* Walking data interface */
- CM730_X_OFFSET = 90,
- CM730_Y_OFFSET = 91,
- CM730_Z_OFFSET = 92,
- CM730_ROLL = 93,
- CM730_PITCH = 94,
- CM730_YAW = 95,
- CM730_HIP_PITCH_OFF_L = 96,
- CM730_HIP_PITCH_OFF_H = 97,
- CM730_PERIOD_TIME_L = 98,
- CM730_PERIOD_TIME_H = 99,
- CM730_DSP_RATIO = 100,
- CM730_STEP_FW_BW_RATIO = 101,
- CM730_STEP_FW_BW = 102,
- CM730_STEP_LEFT_RIGHT = 103,
- CM730_STEP_DIRECTION = 104,
- CM730_FOOT_HEIGHT = 105,
- CM730_SWING_RIGHT_LEFT = 106,
- CM730_SWING_TOP_DOWN = 107,
- CM730_PELVIS_OFFSET = 108,
- CM730_ARM_SWING_GAIN = 109,
- CM730_BAL_KNEE_GAIN = 110,
- CM730_BAL_ANKLE_PITCH_GAIN = 111,
- CM730_BAL_HIP_ROLL_GAIN = 112,
- CM730_BAL_ANKLE_ROLL_GAIN = 113,
- CM730_P_GAIN = 114,
- CM730_D_GAIN = 115,
- CM730_I_GAIN = 116,
-/* joint motion data interface */
- CM730_SERVO_1_POS_L = 117,
- CM730_SERVO_1_POS_H = 118,
- CM730_SERVO_2_POS_L = 119,
- CM730_SERVO_2_POS_H = 120,
- CM730_SERVO_3_POS_L = 121,
- CM730_SERVO_3_POS_H = 122,
- CM730_SERVO_4_POS_L = 123,
- CM730_SERVO_4_POS_H = 124,
- CM730_SERVO_5_POS_L = 125,
- CM730_SERVO_5_POS_H = 126,
- CM730_SERVO_6_POS_L = 127,
- CM730_SERVO_6_POS_H = 128,
- CM730_SERVO_7_POS_L = 129,
- CM730_SERVO_7_POS_H = 130,
- CM730_SERVO_8_POS_L = 131,
- CM730_SERVO_8_POS_H = 132,
- CM730_SERVO_9_POS_L = 133,
- CM730_SERVO_9_POS_H = 134,
- CM730_SERVO_10_POS_L = 135,
- CM730_SERVO_10_POS_H = 136,
- CM730_SERVO_11_POS_L = 137,
- CM730_SERVO_11_POS_H = 138,
- CM730_SERVO_12_POS_L = 139,
- CM730_SERVO_12_POS_H = 140,
- CM730_SERVO_13_POS_L = 141,
- CM730_SERVO_13_POS_H = 142,
- CM730_SERVO_14_POS_L = 143,
- CM730_SERVO_14_POS_H = 144,
- CM730_SERVO_15_POS_L = 145,
- CM730_SERVO_15_POS_H = 146,
- CM730_SERVO_16_POS_L = 147,
- CM730_SERVO_16_POS_H = 148,
- CM730_SERVO_17_POS_L = 149,
- CM730_SERVO_17_POS_H = 150,
- CM730_SERVO_18_POS_L = 151,
- CM730_SERVO_18_POS_H = 152,
- CM730_SERVO_19_POS_L = 153,
- CM730_SERVO_19_POS_H = 154,
- CM730_SERVO_20_POS_L = 155,
- CM730_SERVO_20_POS_H = 156,
- CM730_SERVO_21_POS_L = 157,
- CM730_SERVO_21_POS_H = 158,
- CM730_SERVO_22_POS_L = 159,
- CM730_SERVO_22_POS_H = 160,
- CM730_SERVO_23_POS_L = 161,
- CM730_SERVO_23_POS_H = 162,
- CM730_SERVO_24_POS_L = 163,
- CM730_SERVO_24_POS_H = 164,
- CM730_SERVO_25_POS_L = 165,
- CM730_SERVO_25_POS_H = 166,
- CM730_SERVO_26_POS_L = 167,
- CM730_SERVO_26_POS_H = 168,
- CM730_SERVO_27_POS_L = 169,
- CM730_SERVO_27_POS_H = 170,
- CM730_SERVO_28_POS_L = 171,
- CM730_SERVO_28_POS_H = 172,
- CM730_SERVO_29_POS_L = 173,
- CM730_SERVO_29_POS_H = 174,
- CM730_SERVO_30_POS_L = 175,
- CM730_SERVO_30_POS_H = 176,
- CM730_SERVO_1_SPEED = 177,
- CM730_SERVO_2_SPEED = 178,
- CM730_SERVO_3_SPEED = 179,
- CM730_SERVO_4_SPEED = 180,
- CM730_SERVO_5_SPEED = 181,
- CM730_SERVO_6_SPEED = 182,
- CM730_SERVO_7_SPEED = 183,
- CM730_SERVO_8_SPEED = 184,
- CM730_SERVO_9_SPEED = 185,
- CM730_SERVO_10_SPEED = 186,
- CM730_SERVO_11_SPEED = 187,
- CM730_SERVO_12_SPEED = 188,
- CM730_SERVO_13_SPEED = 189,
- CM730_SERVO_14_SPEED = 190,
- CM730_SERVO_15_SPEED = 191,
- CM730_SERVO_16_SPEED = 192,
- CM730_SERVO_17_SPEED = 193,
- CM730_SERVO_18_SPEED = 194,
- CM730_SERVO_19_SPEED = 195,
- CM730_SERVO_20_SPEED = 196,
- CM730_SERVO_21_SPEED = 197,
- CM730_SERVO_22_SPEED = 198,
- CM730_SERVO_23_SPEED = 199,
- CM730_SERVO_24_SPEED = 200,
- CM730_SERVO_25_SPEED = 201,
- CM730_SERVO_26_SPEED = 202,
- CM730_SERVO_27_SPEED = 203,
- CM730_SERVO_28_SPEED = 204,
- CM730_SERVO_29_SPEED = 205,
- CM730_SERVO_30_SPEED = 206,
- CM730_SERVO_1_ACCEL = 207,
- CM730_SERVO_2_ACCEL = 208,
- CM730_SERVO_3_ACCEL = 209,
- CM730_SERVO_4_ACCEL = 210,
- CM730_SERVO_5_ACCEL = 211,
- CM730_SERVO_6_ACCEL = 212,
- CM730_SERVO_7_ACCEL = 213,
- CM730_SERVO_8_ACCEL = 214,
- CM730_SERVO_9_ACCEL = 215,
- CM730_SERVO_10_ACCEL = 216,
- CM730_SERVO_11_ACCEL = 217,
- CM730_SERVO_12_ACCEL = 218,
- CM730_SERVO_13_ACCEL = 219,
- CM730_SERVO_14_ACCEL = 220,
- CM730_SERVO_15_ACCEL = 221,
- CM730_SERVO_16_ACCEL = 222,
- CM730_SERVO_17_ACCEL = 223,
- CM730_SERVO_18_ACCEL = 224,
- CM730_SERVO_19_ACCEL = 225,
- CM730_SERVO_20_ACCEL = 226,
- CM730_SERVO_21_ACCEL = 227,
- CM730_SERVO_22_ACCEL = 228,
- CM730_SERVO_23_ACCEL = 229,
- CM730_SERVO_24_ACCEL = 230,
- CM730_SERVO_25_ACCEL = 231,
- CM730_SERVO_26_ACCEL = 232,
- CM730_SERVO_27_ACCEL = 233,
- CM730_SERVO_28_ACCEL = 234,
- CM730_SERVO_29_ACCEL = 235,
- CM730_SERVO_30_ACCEL = 236,
- CM730_MODULE_SEL_1 = 237,
- CM730_MODULE_SEL_2 = 238,
- CM730_MODULE_SEL_3 = 239,
- CM730_MODULE_SEL_4 = 240,
- CM730_MODULE_SEL_5 = 241,
- CM730_MODULE_SEL_6 = 242,
- CM730_MODULE_SEL_7 = 243,
- CM730_MODULE_SEL_8 = 244,
- CM730_NUM_SERVOS = 245
-}TRAMReg;
+ DARWIN_MODEL_NUMBER_L = DEVICE_MODEL_OFFSET,
+ DARWIN_MODEL_NUMBER_H = DEVICE_MODEL_OFFSET+1,
+ DARWIN_VERSION = FIRMWARE_VERSION_OFFSET,
+ DARWIN_ID = DEVICE_ID_OFFSET,
+ DARWIN_BAUD_RATE = BAUDRATE_OFFSET,
+ DARWIN_RETURN_DELAY_TIME = RETURN_DELAY_OFFSET,
+ DARWIN_MM_PERIOD = MM_PERIOD_OFFSET,
+ DARWIN_RETURN_LEVEL = RETURN_LEVEL_OFFSET,
+ DARWIN_DXL_POWER = 0x18,
+ DARWIN_LED_PANNEL = 0x19,
+ DARWIN_LED_5_L = 0x1A,
+ DARWIN_LED_5_H = 0x1B,
+ DARWIN_LED_6_L = 0x1C,
+ DARWIN_LED_6_H = 0x1D,
+ DARWIN_PUSHBUTTON = 0x1E,
+ DARWIN_MM_NUM_SERVOS = 0x1F,
+ DARWIN_MM_CONTROL = 0x20,
+ DARWIN_MM_STATUS = 0x21,
+ DARWIN_WALK_CONTROL = 0x22,
+ DARWIN_WALK_STATUS = 0x23,
+ DARWIN_IMU_CONTROL = 0x24,
+ DARWIN_IMU_STATUS = 0x25,
+ DARWIN_IMU_ACCEL_X_L = 0x26,
+ DARWIN_IMU_ACCEL_X_H = 0x27,
+ DARWIN_IMU_ACCEL_Y_L = 0x28,
+ DARWIN_IMU_ACCEL_Y_H = 0x29,
+ DARWIN_IMU_ACCEL_Z_L = 0x2A,
+ DARWIN_IMU_ACCEL_Z_H = 0x2B,
+ DARWIN_IMU_GYRO_X_L = 0x2C,
+ DARWIN_IMU_GYRO_X_H = 0x2D,
+ DARWIN_IMU_GYRO_Y_L = 0x2E,
+ DARWIN_IMU_GYRO_Y_H = 0x2F,
+ DARWIN_IMU_GYRO_Z_L = 0x30,
+ DARWIN_IMU_GYRO_Z_H = 0x31,
+ DARWIN_VOLTAGE = 0x32,
+ DARWIN_MIC1_L = 0x33,
+ DARWIN_MIC1_H = 0x34,
+ DARWIN_ADC2_L = 0x35,
+ DARWIN_ADC2_H = 0x36,
+ DARWIN_ADC3_L = 0x37,
+ DARWIN_ADC3_H = 0x38,
+ DARWIN_ADC4_L = 0x39,
+ DARWIN_ADC4_H = 0x3A,
+ DARWIN_ADC5_L = 0x3B,
+ DARWIN_ADC5_H = 0x3C,
+ DARWIN_ADC6_L = 0x3D,
+ DARWIN_ADC6_H = 0x3E,
+ DARWIN_ADC7_L = 0x3F,
+ DARWIN_ADC7_H = 0x40,
+ DARWIN_ADC8_L = 0x41,
+ DARWIN_ADC8_H = 0x42,
+ DARWIN_MIC2_L = 0x43,
+ DARWIN_MIC2_H = 0x44,
+ DARWIN_ADC10_L = 0x45,
+ DARWIN_ADC10_H = 0x46,
+ DARWIN_ADC11_L = 0x47,
+ DARWIN_ADC11_H = 0x48,
+ DARWIN_ADC12_L = 0x49,
+ DARWIN_ADC12_H = 0x4A,
+ DARWIN_ADC13_L = 0x4B,
+ DARWIN_ADC13_H = 0x4C,
+ DARWIN_ADC14_L = 0x4D,
+ DARWIN_ADC14_H = 0x4E,
+ DARWIN_ADC15_L = 0x4F,
+ DARWIN_ADC15_H = 0x50,
+ DARWIN_MM_MODULE_EN0 = 0x51,
+ DARWIN_MM_MODULE_EN1 = 0x52,
+ DARWIN_MM_MODULE_EN2 = 0x53,
+ DARWIN_MM_MODULE_EN3 = 0x54,
+ DARWIN_MM_MODULE_EN4 = 0x55,
+ DARWIN_MM_MODULE_EN5 = 0x56,
+ DARWIN_MM_MODULE_EN6 = 0x57,
+ DARWIN_MM_MODULE_EN7 = 0x58,
+ DARWIN_MM_MODULE_EN8 = 0x59,
+ DARWIN_MM_MODULE_EN9 = 0x5A,
+ DARWIN_MM_MODULE_EN10 = 0x5B,
+ DARWIN_MM_MODULE_EN11 = 0x5C,
+ DARWIN_MM_MODULE_EN12 = 0x5D,
+ DARWIN_MM_MODULE_EN13 = 0x5E,
+ DARWIN_MM_MODULE_EN14 = 0x5F,
+ DARWIN_MM_MODULE_EN15 = 0x60,
+ DARWIN_MM_SERVO0_CUR_POS_L = 0x61,
+ DARWIN_MM_SERVO0_CUR_POS_H = 0x62,
+ DARWIN_MM_SERVO1_CUR_POS_L = 0x63,
+ DARWIN_MM_SERVO1_CUR_POS_H = 0x64,
+ DARWIN_MM_SERVO2_CUR_POS_L = 0x65,
+ DARWIN_MM_SERVO2_CUR_POS_H = 0x66,
+ DARWIN_MM_SERVO3_CUR_POS_L = 0x67,
+ DARWIN_MM_SERVO3_CUR_POS_H = 0x68,
+ DARWIN_MM_SERVO4_CUR_POS_L = 0x69,
+ DARWIN_MM_SERVO4_CUR_POS_H = 0x6A,
+ DARWIN_MM_SERVO5_CUR_POS_L = 0x6B,
+ DARWIN_MM_SERVO5_CUR_POS_H = 0x6C,
+ DARWIN_MM_SERVO6_CUR_POS_L = 0x6D,
+ DARWIN_MM_SERVO6_CUR_POS_H = 0x6E,
+ DARWIN_MM_SERVO7_CUR_POS_L = 0x6F,
+ DARWIN_MM_SERVO7_CUR_POS_H = 0x70,
+ DARWIN_MM_SERVO8_CUR_POS_L = 0x71,
+ DARWIN_MM_SERVO8_CUR_POS_H = 0x72,
+ DARWIN_MM_SERVO9_CUR_POS_L = 0x73,
+ DARWIN_MM_SERVO9_CUR_POS_H = 0x74,
+ DARWIN_MM_SERVO10_CUR_POS_L = 0x75,
+ DARWIN_MM_SERVO10_CUR_POS_H = 0x76,
+ DARWIN_MM_SERVO11_CUR_POS_L = 0x77,
+ DARWIN_MM_SERVO11_CUR_POS_H = 0x78,
+ DARWIN_MM_SERVO12_CUR_POS_L = 0x79,
+ DARWIN_MM_SERVO12_CUR_POS_H = 0x7A,
+ DARWIN_MM_SERVO13_CUR_POS_L = 0x7B,
+ DARWIN_MM_SERVO13_CUR_POS_H = 0x7C,
+ DARWIN_MM_SERVO14_CUR_POS_L = 0x7D,
+ DARWIN_MM_SERVO14_CUR_POS_H = 0x7E,
+ DARWIN_MM_SERVO15_CUR_POS_L = 0x7F,
+ DARWIN_MM_SERVO15_CUR_POS_H = 0x80,
+ DARWIN_MM_SERVO16_CUR_POS_L = 0x81,
+ DARWIN_MM_SERVO16_CUR_POS_H = 0x82,
+ DARWIN_MM_SERVO17_CUR_POS_L = 0x83,
+ DARWIN_MM_SERVO17_CUR_POS_H = 0x84,
+ DARWIN_MM_SERVO18_CUR_POS_L = 0x85,
+ DARWIN_MM_SERVO18_CUR_POS_H = 0x86,
+ DARWIN_MM_SERVO19_CUR_POS_L = 0x87,
+ DARWIN_MM_SERVO19_CUR_POS_H = 0x88,
+ DARWIN_MM_SERVO20_CUR_POS_L = 0x89,
+ DARWIN_MM_SERVO20_CUR_POS_H = 0x8A,
+ DARWIN_MM_SERVO21_CUR_POS_L = 0x8B,
+ DARWIN_MM_SERVO21_CUR_POS_H = 0x8C,
+ DARWIN_MM_SERVO22_CUR_POS_L = 0x8D,
+ DARWIN_MM_SERVO22_CUR_POS_H = 0x8E,
+ DARWIN_MM_SERVO23_CUR_POS_L = 0x8F,
+ DARWIN_MM_SERVO23_CUR_POS_H = 0x90,
+ DARWIN_MM_SERVO24_CUR_POS_L = 0x91,
+ DARWIN_MM_SERVO24_CUR_POS_H = 0x92,
+ DARWIN_MM_SERVO25_CUR_POS_L = 0x93,
+ DARWIN_MM_SERVO25_CUR_POS_H = 0x94,
+ DARWIN_MM_SERVO26_CUR_POS_L = 0x95,
+ DARWIN_MM_SERVO26_CUR_POS_H = 0x96,
+ DARWIN_MM_SERVO27_CUR_POS_L = 0x97,
+ DARWIN_MM_SERVO27_CUR_POS_H = 0x98,
+ DARWIN_MM_SERVO28_CUR_POS_L = 0x99,
+ DARWIN_MM_SERVO28_CUR_POS_H = 0x9A,
+ DARWIN_MM_SERVO29_CUR_POS_L = 0x9B,
+ DARWIN_MM_SERVO29_CUR_POS_H = 0x9C,
+ DARWIN_MM_SERVO30_CUR_POS_L = 0x9D,
+ DARWIN_MM_SERVO30_CUR_POS_H = 0x9E,
+ DARWIN_ACTION_PAGE = 0x9F,
+ DARWIN_ACTION_CONTROL = 0xA0,
+ DARWIN_ACTION_STATUS = 0xA1,
+ DARWIN_X_OFFSET = 0xA2,
+ DARWIN_Y_OFFSET = 0xA3,
+ DARWIN_Z_OFFSET = 0xA4,
+ DARWIN_ROLL = 0xA5,
+ DARWIN_PITCH = 0xA6,
+ DARWIN_YAW = 0xA7,
+ DARWIN_HIP_PITCH_OFF_L = 0xA8,
+ DARWIN_HIP_PITCH_OFF_H = 0xA9,
+ DARWIN_PERIOD_TIME_L = 0xAA,
+ DARWIN_PERIOD_TIME_H = 0xAB,
+ DARWIN_DSP_RATIO = 0xAC,
+ DARWIN_STEP_FW_BW_RATIO = 0xAD,
+ DARWIN_STEP_FW_BW = 0xAE,
+ DARWIN_STEP_LEFT_RIGHT = 0xAF,
+ DARWIN_STEP_DIRECTION = 0xB1,
+ DARWIN_FOOT_HEIGHT = 0xB2,
+ DARWIN_SWING_RIGHT_LEFT = 0xB3,
+ DARWIN_SWING_TOP_DOWN = 0xB4,
+ DARWIN_PELVIS_OFFSET = 0xB5,
+ DARWIN_ARM_SWING_GAIN = 0xB6,
+ DARWIN_BAL_KNEE_GAIN = 0xB7,
+ DARWIN_BAL_ANKLE_PITCH_GAIN = 0xB8,
+ DARWIN_BAL_HIP_ROLL_GAIN = 0xB9,
+ DARWIN_BAL_ANKLE_ROLL_GAIN = 0xBA,
+ DARWIN_P_GAIN = 0xBB,
+ DARWIN_D_GAIN = 0xBC,
+ DARWIN_I_GAIN = 0xBD
+} darwin_registers;
+
+#define RAM_SIZE 256
+
+extern uint8_t ram_data[RAM_SIZE];
-// public functions
void ram_init(void);
inline void ram_read_byte(uint8_t address, uint8_t *data);
inline void ram_read_word(uint8_t address, uint16_t *data);
@@ -262,5 +199,6 @@ uint8_t ram_clear_bit(uint8_t address, uint8_t bit);
uint8_t ram_write_byte(uint8_t address, uint8_t data);
uint8_t ram_write_word(uint8_t address, uint16_t data);
uint8_t ram_write_table(uint8_t address, uint8_t length,uint8_t *data);
+inline uint8_t ram_in_range(uint8_t start_reg,uint8_t end_reg,uint8_t address,uint8_t length);
#endif
diff --git a/include/spi.h b/include/spi.h
deleted file mode 100755
index 9d0d28102975a06cdf9284a90a3f92ebc11e980d..0000000000000000000000000000000000000000
--- a/include/spi.h
+++ /dev/null
@@ -1,13 +0,0 @@
-#ifndef _SPI_H
-#define _SPI_H
-
-#include "stm32f10x.h"
-
-//public functions
-void spi_init(void);
-void spi_read_byte(uint8_t address, uint8_t *data);
-void spi_read_word(uint8_t address, uint16_t *data);
-void spi_write_byte(uint8_t address, uint8_t data);
-void spi_write_word(uint8_t address, uint16_t data);
-
-#endif
diff --git a/include/stm32f10x_it.h b/include/stm32f10x_it.h
deleted file mode 100755
index 0fa4a08401a2ca55b35fe5352fe99a773e0431eb..0000000000000000000000000000000000000000
--- a/include/stm32f10x_it.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/**
- ******************************************************************************
- * @file Project/STM32F10x_StdPeriph_Template/stm32f10x_it.h
- * @author MCD Application Team
- * @version V3.5.0
- * @date 08-April-2011
- * @brief This file contains the headers of the interrupt handlers.
- ******************************************************************************
- * @attention
- *
- * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
- * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
- * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
- * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
- * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
- * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
- *
- * <h2><center>© COPYRIGHT 2011 STMicroelectronics</center></h2>
- ******************************************************************************
- */
-
-/* Define to prevent recursive inclusion -------------------------------------*/
-#ifndef __STM32F10x_IT_H
-#define __STM32F10x_IT_H
-
-#ifdef __cplusplus
- extern "C" {
-#endif
-
-/* Includes ------------------------------------------------------------------*/
-#include "stm32f10x.h"
-
-// NMI interrupt handler
-void NMI_Handler(void);
-void HardFault_Handler(void);
-void MemManage_Handler(void);
-void BusFault_Handler(void);
-void UsageFault_Handler(void);
-void SVC_Handler(void);
-void DebugMon_Handler(void);
-void PendSV_Handler(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* __STM32F10x_IT_H */
-
diff --git a/include/system_init.h b/include/system_init.h
deleted file mode 100755
index 6ae35eaf0b8c5357935ab0c99ae049926f82790f..0000000000000000000000000000000000000000
--- a/include/system_init.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#ifndef _SYSTEM_INIT_H
-#define _SYSTEM_INIT_H
-
-#include "stm32f10x.h"
-
-void clocks_init(void);
-
-#endif
diff --git a/include/utils.h b/include/utils.h
deleted file mode 100755
index 2bd5cf035e027c22d7226a053346d96c95ba940c..0000000000000000000000000000000000000000
--- a/include/utils.h
+++ /dev/null
@@ -1,10 +0,0 @@
-#ifndef _UTILS_H
-#define _UTILS_H
-
-#include "stm32f10x.h"
-
-inline uint16_t make_word(uint8_t msb,uint8_t lsb);
-inline uint8_t get_msb(uint16_t data);
-inline uint8_t get_lsb(uint16_t data);
-
-#endif
diff --git a/include/vector.h b/include/vector.h
deleted file mode 100755
index ff316056e705c4447aa08b7f0be2ebd9368a1dc7..0000000000000000000000000000000000000000
--- a/include/vector.h
+++ /dev/null
@@ -1,17 +0,0 @@
-#ifndef _VECTOR_H_
-#define _VECTOR_H_
-
-#include "point.h"
-
-typedef struct {
- float x;
- float y;
- float z;
-}TVector3D;
-
-void vector3d_init(TVector3D *vector);
-void vector3d_load(TVector3D *vector, float x, float y, float z);
-void vector3d_copy(TVector3D *vector_dst, TVector3D *vector_src);
-float vector3d_length(TVector3D *vector);
-
-#endif
diff --git a/include/walking.h b/include/walking.h
index a71b3d7270b66c172bbc9c32303c0df6304b0e68..0b7e4773c19c089450704b25897cc6a1fe9bd8d7 100755
--- a/include/walking.h
+++ b/include/walking.h
@@ -2,6 +2,9 @@
#define _WALKING_H
#include "stm32f10x.h"
+#include "motion_manager.h"
+
+extern int64_t walking_current_angles[MANAGER_MAX_NUM_SERVOS];
// public functions
void walking_init(void);
diff --git a/linker_script/darwin.ld b/linker_script/darwin.ld
new file mode 100755
index 0000000000000000000000000000000000000000..8aba769649d374f6be65ee522cb12d639795732b
--- /dev/null
+++ b/linker_script/darwin.ld
@@ -0,0 +1,159 @@
+/*
+*****************************************************************************
+**
+*****************************************************************************
+*/
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/* Highest address of the user mode stack */
+_estack = 0x2000FFFF; /* end of RAM */
+
+/* Generate a link error if heap and stack don't fit into RAM */
+_Min_Heap_Size = 0; /* required amount of heap */
+_Min_Stack_Size = 0x400; /* required amount of stack */
+
+/* Specify the memory areas */
+MEMORY
+{
+ISR (rx) : ORIGIN = 0x08003000, LENGTH = 2K
+EEPROM (rw) : ORIGIN = 0x08003800, LENGTH = 4K
+PAGES (r) : ORIGIN = 0x08004800, LENGTH = 24K
+FLASH (rx) : ORIGIN = 0x0800A800, LENGTH = 470K
+RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 64K
+}
+
+/* Define output sections */
+SECTIONS
+{
+ /* The startup code goes first into FLASH */
+ .isr_vector :
+ {
+ . = ALIGN(4);
+ KEEP(*(.isr_vector)) /* Startup code */
+ . = ALIGN(4);
+ } >ISR
+
+ .eeprom :
+ {
+ . = ALIGN(4);
+ *(.eeprom)
+ . = ALIGN(4);
+ } >EEPROM
+
+ /* The program code and other data goes into FLASH */
+ .text :
+ {
+ . = ALIGN(4);
+ *(.text) /* .text sections (code) */
+ *(.text*) /* .text* sections (code) */
+ *(.glue_7) /* glue arm to thumb code */
+ *(.glue_7t) /* glue thumb to arm code */
+ *(.eh_frame)
+
+ KEEP (*(.init))
+ KEEP (*(.fini))
+
+ . = ALIGN(4);
+ _etext = .; /* define a global symbols at end of code */
+ } >FLASH
+
+ /* Constant data goes into FLASH */
+ .rodata :
+ {
+ . = ALIGN(4);
+ *(.rodata) /* .rodata sections (constants, strings, etc.) */
+ *(.rodata*) /* .rodata* sections (constants, strings, etc.) */
+ . = ALIGN(4);
+ } >FLASH
+
+ .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH
+ .ARM : {
+ __exidx_start = .;
+ *(.ARM.exidx*)
+ __exidx_end = .;
+ } >FLASH
+
+ .preinit_array :
+ {
+ PROVIDE_HIDDEN (__preinit_array_start = .);
+ KEEP (*(.preinit_array*))
+ PROVIDE_HIDDEN (__preinit_array_end = .);
+ } >FLASH
+ .init_array :
+ {
+ PROVIDE_HIDDEN (__init_array_start = .);
+ KEEP (*(SORT(.init_array.*)))
+ KEEP (*(.init_array*))
+ PROVIDE_HIDDEN (__init_array_end = .);
+ } >FLASH
+ .fini_array :
+ {
+ PROVIDE_HIDDEN (__fini_array_start = .);
+ KEEP (*(SORT(.fini_array.*)))
+ KEEP (*(.fini_array*))
+ PROVIDE_HIDDEN (__fini_array_end = .);
+ } >FLASH
+
+ /* used by the startup to initialize data */
+ _sidata = LOADADDR(.data);
+
+ /* Initialized data sections goes into RAM, load LMA copy after code */
+ .data :
+ {
+ . = ALIGN(4);
+ _sdata = .; /* create a global symbol at data start */
+ *(.data) /* .data sections */
+ *(.data*) /* .data* sections */
+
+ . = ALIGN(4);
+ _edata = .; /* define a global symbol at data end */
+ } >RAM AT> FLASH
+
+ .pages :
+ {
+ . = ALIGN(4);
+ *(.pages)
+ . = ALIGN(4);
+ } >PAGES
+
+ /* Uninitialized data section */
+ . = ALIGN(4);
+ .bss :
+ {
+ /* This is used by the startup in order to initialize the .bss secion */
+ _sbss = .; /* define a global symbol at bss start */
+ __bss_start__ = _sbss;
+ *(.bss)
+ *(.bss*)
+ *(COMMON)
+
+ . = ALIGN(4);
+ _ebss = .; /* define a global symbol at bss end */
+ __bss_end__ = _ebss;
+ } >RAM
+
+ /* User_heap_stack section, used to check that there is enough RAM left */
+ ._user_heap_stack :
+ {
+ . = ALIGN(4);
+ PROVIDE ( end = . );
+ PROVIDE ( _end = . );
+ . = . + _Min_Heap_Size;
+ . = . + _Min_Stack_Size;
+ . = ALIGN(4);
+ } >RAM
+
+
+
+ /* Remove information from the standard libraries */
+ /DISCARD/ :
+ {
+ libc.a ( * )
+ libm.a ( * )
+ libgcc.a ( * )
+ }
+
+ .ARM.attributes 0 : { *(.ARM.attributes) }
+}
diff --git a/src/action.c b/src/action.c
index 0f929cc5922b4780d138cd4f67a5395feb0e350e..cc0429d99bb803ffc144cbd5a5e4ae280e5ed584 100755
--- a/src/action.c
+++ b/src/action.c
@@ -1,446 +1,502 @@
#include "action.h"
-#include "dyn_servos.h"
#include "motion_pages.h"
-#include "motion_manager.h"
#include "ram.h"
-/**************************************
- * Section /----\
- * /| |\
- * /+---------/ | | \
- * / | | | | \
- * -----/ | | | | \----
- * PRE MAIN PRE MAIN POST PAUSE
- ***************************************/
-typedef enum{ PRE_SECTION, MAIN_SECTION, POST_SECTION, PAUSE_SECTION } TSection;
-typedef enum{ ZERO_FINISH, NONE_ZERO_FINISH} TStopMode;
-
-#define SPEED_BASE_SCHEDULE 0x00
-#define TIME_BASE_SCHEDULE 0x0a
-#define INVALID_BIT_MASK 0x4000
+#define SPEED_BASE_SCHEDULE 0x00
+#define TIME_BASE_SCHEDULE 0x0A
// private variables
-uint8_t action_finished;
-uint8_t action_stop;
-uint8_t action_next_index;
-uint8_t action_current_index;
-uint8_t action_step_count;
+typedef enum {ACTION_PRE,ACTION_MAIN,ACTION_POST,ACTION_PAUSE} action_states;
+
TPage action_next_page;
TPage action_current_page;
-
-// motion variables
-uint16_t wpStartAngle1024[PAGE_MAX_NUM_SERVOS];
-uint16_t wpTargetAngle1024[PAGE_MAX_NUM_SERVOS];
-int16_t ipMovingAngle1024[PAGE_MAX_NUM_SERVOS];
-int16_t ipMainAngle1024[PAGE_MAX_NUM_SERVOS];
-int16_t ipAccelAngle1024[PAGE_MAX_NUM_SERVOS];
-int16_t ipMainSpeed1024[PAGE_MAX_NUM_SERVOS];
-int16_t ipLastOutSpeed1024[PAGE_MAX_NUM_SERVOS];
-int16_t ipGoalSpeed1024[PAGE_MAX_NUM_SERVOS];
-uint8_t bpFinishType[PAGE_MAX_NUM_SERVOS];
-uint16_t wUnitTimeCount;
-uint16_t wUnitTimeNum;
-uint16_t wPauseTime;
-uint16_t wUnitTimeTotalNum;
-uint16_t wAccelStep;
-uint8_t bSection;
-uint8_t bPlayRepeatCount;
-uint16_t step_angles[PAGE_MAX_NUM_SERVOS];
-
-// public functions
-void action_init(void)
-{
- ram_write_byte(CM730_ACTION_STATUS,0x00);
- action_finished=0x00;
- action_stop=0x00;
- action_next_index=0xFF;
- action_current_index=0xFF;
- action_step_count=0xFF;
-}
-
-uint8_t action_set_page(uint8_t page_id)
-{
- uint8_t error;
-
- error=load_page_info(page_id,&action_current_page);
- if(error==PAGE_SUCCESS)
- {
- ram_write_byte(CM730_ACTION_NUMBER,page_id);
- action_current_index=page_id;
- }
-
- return error;
-}
-
-void action_start_page(void)
-{
- uint8_t i;
-
- action_finished=0x00;// reset the finished flag
- wUnitTimeCount = 0;
- wUnitTimeNum = 0;
- wPauseTime = 0;
- bSection = PAUSE_SECTION;
- action_step_count = 0;
- bPlayRepeatCount = action_current_page.header.repeat;
- action_next_index = 0x00;
-
- for( i=0; i<manager_get_num_servos(); i++ )
- {
- if(manager_get_index_module(i)==MM_ACTION)
- {
- wpTargetAngle1024[i] = manager_get_index_value(i);
- ipLastOutSpeed1024[i] = 0;
- ipMovingAngle1024[i] = 0;
- ipGoalSpeed1024[i] = 0;
- }
- }
- ram_write_byte(CM730_ACTION_STATUS,0x01);
- ram_set_bit(CM730_ACTION_CONTROL,0x00);
- ram_clear_bit(CM730_ACTION_CONTROL,0x01);
-}
-
-void action_stop_page(void)
-{
- action_stop=0x01;
- ram_clear_bit(CM730_ACTION_CONTROL,0x00);
- ram_set_bit(CM730_ACTION_CONTROL,0x01);
-}
-
-uint8_t action_is_running(void)
-{
- uint8_t running;
-
- ram_read_byte(CM730_ACTION_STATUS,&running);
-
- return running;
-}
-
-// motion manager interface
-void action_process(void)
+uint8_t action_current_step_index;
+// angle variables
+int64_t action_moving_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+int64_t action_current_angles[PAGE_MAX_NUM_SERVOS];
+int64_t action_start_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+// speed variables
+int64_t action_start_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+int64_t action_main_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+// control variables
+uint8_t action_end,action_stop;
+uint8_t action_zero_speed_finish[PAGE_MAX_NUM_SERVOS];
+uint8_t action_next_index;
+uint8_t action_running;
+// time variables (in time units (7.8 ms each time unit))
+int64_t action_total_time;// fixed point 48|16 format
+int64_t action_pre_time;// fixed point 48|16 format
+int64_t action_main_time;// fixed point 48|16 format
+int64_t action_step_time;// fixed point 48|16 format
+int64_t action_pause_time;// fixed point 48|16 format
+int64_t action_current_time;// fixed point 48|16 format
+int64_t action_section_time;// fixed point 48|16 format
+int64_t action_period;
+
+// private functions
+void action_load_next_step(void)
{
- //////////////////// Ãö¿ª º¯¼ö
- uint8_t bID;
- uint64_t ulTotalTime256T;
- uint64_t ulPreSectionTime256T;
- uint64_t ulMainTime256T;
- int64_t lStartSpeed1024_PreTime_256T;
- int64_t lMovingAngle_Speed1024Scale_256T_2T;
- int64_t lDivider1,lDivider2;
- uint16_t wMaxAngle1024;
- uint16_t wMaxSpeed256;
- uint16_t wPrevTargetAngle; // Start position
- uint16_t wCurrentTargetAngle; // Target position
- uint16_t wNextTargetAngle; // Next target position
- uint8_t bDirectionChanged;
- int16_t iSpeedN;
-
- if(action_is_running() == 0x00 )
- return;
- if( wUnitTimeCount < wUnitTimeNum ) // ÇöÀç ÃøÇà ÃßÀ̶ó¸é
+ // page and step information
+ static uint8_t current_index=0;
+ static int8_t num_repetitions=0;
+ // angle variables
+ int16_t next_angle;// information from the flash memory (fixed point 9|7 format)
+ int64_t max_angle;// fixed point format 48|16 format
+ int64_t tmp_angle;// fixed point format 48|16 format
+ int64_t angle;// fixed point format 48|16 format
+ int64_t target_angles;// fixed point 48|16 format
+ int64_t next_angles;// fixed point 48|16 format
+ int64_t action_pre_time_initial_speed_angle;// fixed point 48|16 format
+ int64_t moving_angle;// fixed point 48|16 format
+ // time variables
+ int64_t accel_time_num;// fixed point 48|16 format
+ int64_t zero_speed_divider;// fixed point 48|16 format
+ int64_t non_zero_speed_divider;// fixed point 48|16 format
+ // control variables
+ uint8_t dir_change;
+ // control information
+ uint8_t i=0;
+
+ action_current_step_index++;
+ if(action_current_step_index>=action_current_page.header.stepnum)// the last step has been executed
{
- wUnitTimeCount++;
- if( bSection == PAUSE_SECTION )
- {
- }
- else
+ if(action_current_page.header.stepnum==1)
{
- for(bID=1;bID<manager_get_num_servos();bID++)
+ if(action_stop)
+ action_next_index=action_current_page.header.exit;
+ else
{
- if(manager_get_index_module(bID) == MM_ACTION)
+ num_repetitions--;
+ if(num_repetitions>0)
+ action_next_index=current_index;
+ else
+ action_next_index=action_current_page.header.next;
+ }
+ if(action_next_index==0)
+ action_end=0x01;
+ else
+ {
+ if(action_next_index!=current_index)
{
- if( ipMovingAngle1024[bID] == 0 )
- manager_set_servo_value(bID,wpStartAngle1024[bID]);
- else
- {
- if( bSection == PRE_SECTION )
- {
- iSpeedN = (short)(((long)(ipMainSpeed1024[bID] - ipLastOutSpeed1024[bID]) * wUnitTimeCount) / wUnitTimeNum);
- ipGoalSpeed1024[bID] = ipLastOutSpeed1024[bID] + iSpeedN;
- ipAccelAngle1024[bID] = (short)((((long)(ipLastOutSpeed1024[bID] + (iSpeedN >> 1)) * wUnitTimeCount * 144) / 15) >> 9);
- manager_set_servo_value(bID,wpStartAngle1024[bID] + ipAccelAngle1024[bID]);
- }
- else if( bSection == MAIN_SECTION )
- {
- manager_set_servo_value(bID,wpStartAngle1024[bID] + (short int)(((long)(ipMainAngle1024[bID])*wUnitTimeCount) / wUnitTimeNum));
- ipGoalSpeed1024[bID] = ipMainSpeed1024[bID];
- }
- else // POST_SECTION
- {
- if( wUnitTimeCount == (wUnitTimeNum-1) )
- {
- // ½ºÅÜ ¸¶Ãö¸· ¿ÀÂ÷¸¦ ÃÙÀ̱âÀ§ÇØ ±×³É ¸ñÇ¥ À§Ä¡ °ªÀ» »ç¿ë
- manager_set_servo_value(bID,wpTargetAngle1024[bID]);
- }
- else
- {
- if( bpFinishType[bID] == ZERO_FINISH )
- {
- iSpeedN = (short int)(((long)(0 - ipLastOutSpeed1024[bID]) * wUnitTimeCount) / wUnitTimeNum);
- ipGoalSpeed1024[bID] = ipLastOutSpeed1024[bID] + iSpeedN;
- manager_set_servo_value(bID,wpStartAngle1024[bID] + (short)((((long)(ipLastOutSpeed1024[bID] + (iSpeedN>>1)) * wUnitTimeCount * 144) / 15) >> 9));
- }
- else // NONE_ZERO_FINISH
- {
- // MAIN Section°ú µ¿ÀÃÇÃ°Ô ÀÛµ¿-µ¿ÀÃ
- // step¿¡¼ ¾î¶²¼Âº¸´Â °¡°à ¾î¶² ¼Âº¸´Â ¼Â¾ßÇô »óȲÀÌ ¹ß»ýÇÒ ¼ö ÀÖÀ¸¹Ç·Î ÀÌ·¸°Ô ÇÒ ¼ö¹Û¿¡ ¾øÀ½
- manager_set_servo_value(bID,wpStartAngle1024[bID] + (short int)(((long)(ipMainAngle1024[bID]) * wUnitTimeCount) / wUnitTimeNum));
- ipGoalSpeed1024[bID] = ipMainSpeed1024[bID];
- }
- }
- }
- }
+ pages_get_page(action_next_index,&action_next_page);
+ if(!pages_check_checksum(&action_next_page))
+ action_end=0x01;
}
+ if(action_next_page.header.repeat==0 || action_next_page.header.stepnum==0)
+ action_end=0x01;
}
}
+ pages_copy_page(&action_next_page,&action_current_page);// make the next page active
+ if(current_index!=action_next_index)
+ num_repetitions=action_current_page.header.repeat;
+ action_current_step_index=0;
+ current_index=action_next_index;
}
- else if( wUnitTimeCount >= wUnitTimeNum ) // ÇöÀç SectionÀÌ ¿Ã·áµÇ¾ú´Ù¸é
+ else if(action_current_step_index==action_current_page.header.stepnum-1)// it is the last step
{
- wUnitTimeCount = 0;
-
- for(bID=1;bID<manager_get_num_servos();bID++)
+ if(action_stop)
+ action_next_index=action_current_page.header.exit;
+ else
{
- if(manager_get_index_module(bID) == MM_ACTION)
- {
- wpStartAngle1024[bID] = manager_get_servo_value(bID);
- ipLastOutSpeed1024[bID] = ipGoalSpeed1024[bID];
- }
+ num_repetitions--;
+ if(num_repetitions>0)
+ action_next_index=current_index;
+ else
+ action_next_index=action_current_page.header.next;
}
-
- // Section ¾÷µ¥ÀÌÆ® ( PRE -> MAIN -> POST -> (PAUSE or PRE) ... )
- if( bSection == PRE_SECTION )
+ if(action_next_index==0)
+ action_end=0x01;
+ else
{
- // MAIN Section Ãغñ
- bSection = MAIN_SECTION;
- wUnitTimeNum = wUnitTimeTotalNum - (wAccelStep << 1);
- for(bID=1;bID<manager_get_num_servos();bID++)
+ if(action_next_index!=current_index)
{
- if(manager_get_index_module(bID) == MM_ACTION)
- {
- if( bpFinishType[bID] == NONE_ZERO_FINISH )
- {
- if( (wUnitTimeTotalNum - wAccelStep) == 0 ) // µî¼Ó ±¸°£ÀÌ ÀüÇô ¾ø´Ù¸é
- ipMainAngle1024[bID] = 0;
- else
- ipMainAngle1024[bID] = (short)((((long)(ipMovingAngle1024[bID] - ipAccelAngle1024[bID])) * wUnitTimeNum) / (wUnitTimeTotalNum - wAccelStep));
- }
- else // ZERO_FINISH
- ipMainAngle1024[bID] = ipMovingAngle1024[bID] - ipAccelAngle1024[bID] - (short int)((((long)ipMainSpeed1024[bID] * wAccelStep * 12) / 5) >> 8);
- }
+ pages_get_page(action_next_index,&action_next_page);
+ if(!pages_check_checksum(&action_next_page))
+ action_end=0x01;
}
+ if(action_next_page.header.repeat==0 || action_next_page.header.stepnum==0)
+ action_end=0x01;
}
- else if( bSection == MAIN_SECTION )
+ }
+ // compute trajectory
+ action_pause_time=((action_current_page.steps[action_current_step_index].pause<<14)/action_current_page.header.speed);
+ action_step_time=((action_current_page.steps[action_current_step_index].time<<14)/action_current_page.header.speed);
+ if(action_step_time<action_period)
+ action_step_time=action_period;// 0.078 in 16|16 format
+ max_angle=0;
+ for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
+ {
+ angle=action_current_page.steps[action_current_step_index].position[i];
+ if(angle==0x5A00)// bigger than 180
+ target_angles=action_current_angles[i];
+ else
+ target_angles=angle<<9;
+ action_moving_angles[i]=target_angles-action_current_angles[i];
+ if(action_end)
+ next_angles=target_angles;
+ else
{
- // POST Section Ãغñ
- bSection = POST_SECTION;
- wUnitTimeNum = wAccelStep;
-
- for(bID=1;bID<manager_get_num_servos();bID++)
- {
- if(manager_get_index_module(bID) == MM_ACTION)
- ipMainAngle1024[bID] = ipMovingAngle1024[bID] - ipMainAngle1024[bID] - ipAccelAngle1024[bID];
- }
+ if(action_current_step_index==action_current_page.header.stepnum-1)
+ next_angle=action_next_page.steps[0].position[i];
+ else
+ next_angle=action_current_page.steps[action_current_step_index+1].position[i];
+ if(next_angle==0x5A00)
+ next_angles=target_angles;
+ else
+ next_angles=next_angle<<9;
}
- else if( bSection == POST_SECTION )
+ // check changes in direction
+ if(((action_current_angles[i] < target_angles) && (target_angles < next_angles)) ||
+ ((action_current_angles[i] > target_angles) && (target_angles > next_angles)))
+ dir_change=0x00;
+ else
+ dir_change=0x01;
+ // check the type of ending
+ if(dir_change || action_pause_time>0 || action_end)
+ action_zero_speed_finish[i]=0x01;
+ else
+ action_zero_speed_finish[i]=0x00;
+ // find the maximum angle of motion in speed base schedule
+ if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)// speed base schedule
{
- // Pause time À¯¹«¿¡µû¶ó ´Þ¶óÃü
- if( wPauseTime )
- {
- bSection = PAUSE_SECTION;
- wUnitTimeNum = wPauseTime;
- }
+ // fer el valor absolut
+ if(action_moving_angles[i]<0)
+ tmp_angle=-action_moving_angles[i];
else
- {
- bSection = PRE_SECTION;
- }
+ tmp_angle=action_moving_angles[i];
+ if(tmp_angle>max_angle)
+ max_angle=tmp_angle;
}
- else if( bSection == PAUSE_SECTION )
+ }
+ if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)
+ action_step_time=(max_angle*256)/(action_current_page.steps[action_current_step_index].time*720);
+ accel_time_num=action_current_page.header.accel;
+ accel_time_num=accel_time_num<<9;
+ if(action_step_time<=(accel_time_num<<1))
+ {
+ if(action_step_time==0)
+ accel_time_num=0;
+ else
{
- // PRE Section Ãغñ
- bSection = PRE_SECTION;
-
- for(bID=1;bID<manager_get_num_servos();bID++)
- {
- if(manager_get_index_module(bID) == MM_ACTION)
- ipLastOutSpeed1024[bID] = 0;
- }
+ accel_time_num=(action_step_time-action_period)>>1;
+ if(accel_time_num==0)
+ action_step_time=0;
}
+ }
+ action_total_time=action_step_time;
+ action_pre_time=accel_time_num;
+ action_main_time=action_total_time-action_pre_time;
+ non_zero_speed_divider=(action_pre_time>>1)+action_main_time;
+ if(non_zero_speed_divider<action_period)
+ non_zero_speed_divider=action_period;
+ zero_speed_divider=action_main_time;
+ if(zero_speed_divider<action_period)
+ zero_speed_divider=action_period;
+ for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
+ {
+ action_pre_time_initial_speed_angle=(action_start_speed[i]*action_pre_time)>>1;
+ moving_angle=action_moving_angles[i]<<16;
+ if(action_zero_speed_finish[i])
+ action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/zero_speed_divider;
+ else
+ action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/non_zero_speed_divider;
+// if((action_main_speed[i]>>16)>info.max_speed)
+// action_main_speed[i]=(info.max_speed*65536);
+// else if((action_main_speed[i]>>16)<-info.max_speed)
+// action_main_speed[i]=-(info.max_speed*65536);
+ }
+}
- // PRE Section½Ã¿¡ ¸ðµç Ãغñ¸¦ ÇÑ´Ù.
- if( bSection == PRE_SECTION )
- {
- if( action_finished == 0x01 ) // ¸ð¼ÇÀÌ ³¡³µ´Ù¸é
- {
- return;
- }
- action_step_count++;
-
- if( action_step_count > action_current_page.header.stepnum ) // ÇöÀç ÆäÀÌÃö Àç»ýÀÌ ³¡³µ´Ù¸é
- {
- action_current_page = action_next_page;
- if( action_current_index != action_next_index )
- bPlayRepeatCount = action_current_page.header.repeat;
- action_step_count = 1;
- action_current_index = action_next_index;
- }
- if( action_step_count == action_current_page.header.stepnum ) // ¸¶Ãö¸· ½ºÅÜÀ̶ó¸é
- {
- // ´ÙÀ½ ÆäÀÌÃö ·Îµù
- if( action_stop == 0x01 ) // ¸ð¼Ç äÃö ¸Ã·ÉÀÌ ÀÖ´Ù¸é
- {
- action_next_index = action_current_page.header.exit; // ´ÙÀ½ ÆäÀÌÃö´Â Exit ÆäÀÌÃö·Î
- }
- else
- {
- bPlayRepeatCount--;
- if( bPlayRepeatCount > 0 ) // ¹Ãº¹ Ƚ¼ö°¡ ³²¾Ò´Ù¸é
- action_next_index = action_current_index; // ´ÙÀ½ ÆäÀÌÃö´Â ÇöÀç ÆäÀÌÃö·Î
- else // ¹Ãº¹À» ´ÙÇß´Ù¸é
- action_next_index = action_current_page.header.next; // ´ÙÀ½ ÆäÀÌÃö´Â Next ÆäÀÌÃö·Î
- }
-
- if( action_next_index == 0x00 ) // Àç»ýÇÒ ´ÙÀ½ ÆäÀÌÃö°¡ ¾ø´Ù¸é ÇöÀç ½ºÅܱîÃöÇðà þ·á
- action_finished=0x01;
- else
- {
- // ´ÙÀ½ÆäÀÌÃö ·Îµù(°°À¸¸é ¸Þ¸ð¸® º¹»ç, ´Ù¸£¸é ÆÄÀà Àñâ)
- if( action_current_index != action_next_index )
- load_page_info(action_next_index,&action_next_page);
- else
- action_next_page = action_current_page;
-
- // Àç»ýÇÒ Ã¤º¸°¡ ¾ø´Ù¸é ÇöÀç ½ºÅܱîÃöÇðà þ·á
- if( action_next_page.header.repeat == 0 || action_next_page.header.stepnum == 0 )
- action_finished = 0x01;
- }
- }
- //////// Step ÆĶó¹ÌÅà °è»ê
- wPauseTime = (((unsigned short)action_current_page.steps[action_step_count-1].pause) << 5) / action_current_page.header.speed;
- wMaxSpeed256 = ((unsigned short)action_current_page.steps[action_step_count-1].time * (unsigned short)action_current_page.header.speed) >> 5;
- if( wMaxSpeed256 == 0 )
- wMaxSpeed256 = 1;
- wMaxAngle1024 = 0;
-
- ////////// Jointº° ÆĶó¹ÌÅà °è»ê
- for(bID=1;bID<manager_get_num_servos();bID++)
- {
- if(manager_get_index_module(bID) == MM_ACTION)
- {
- // ÀÌÀü, ÇöÀç, ¹Ì·¡¸¦ ¹ÙÅÃÀ¸·Î ±ËÀûÀ» °è»ê
- ipAccelAngle1024[bID] = 0;
-
- // Find current target angle
- if( action_current_page.steps[action_step_count-1].position[bID] & INVALID_BIT_MASK )
- wCurrentTargetAngle = wpTargetAngle1024[bID];
- else
- wCurrentTargetAngle = action_current_page.steps[action_step_count-1].position[bID];
-
- // Update start, prev_target, curr_target
- wpStartAngle1024[bID] = wpTargetAngle1024[bID];
- wPrevTargetAngle = wpTargetAngle1024[bID];
- wpTargetAngle1024[bID] = wCurrentTargetAngle;
-
- // Find Moving offset
- ipMovingAngle1024[bID] = (int)(wpTargetAngle1024[bID] - wpStartAngle1024[bID]);
+// public functions
+void action_init(uint16_t period)
+{
+ unsigned char i;
- // Find Next target angle
- if( action_step_count == action_current_page.header.stepnum ) // ÇöÀç ½ºÅÜÀÌ ¸¶Ãö¸·À̶ó¸é
- {
- if( action_finished == 0x01 ) // ³¡³¯ ¿¹Ã¤À̶ó¸é
- wNextTargetAngle = wCurrentTargetAngle;
- else
- {
- if( action_next_page.steps[0].position[bID] & INVALID_BIT_MASK )
- wNextTargetAngle = wCurrentTargetAngle;
- else
- wNextTargetAngle = action_next_page.steps[0].position[bID];
- }
- }
- else
- {
- if( action_current_page.steps[action_step_count].position[bID] & INVALID_BIT_MASK )
- wNextTargetAngle = wCurrentTargetAngle;
- else
- wNextTargetAngle = action_current_page.steps[action_step_count].position[bID];
- }
+ // init current_angles with the current position of the servos
+ for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
+ {
+ // angle variables
+ action_moving_angles[i]=0;// fixed point 48|16 format
+ action_start_angles[i]=action_current_angles[i];
+ // speed variables
+ action_start_speed[i]=0;// fixed point 48|16 format
+ action_main_speed[i]=0;// fixed point 48|16 format
+ // control variables
+ action_zero_speed_finish[i]=0x00;
+ }
+ // clear the contents of the next page
+ pages_clear_page(&action_next_page);
+ pages_clear_page(&action_current_page);
+ // control variables
+ action_end=0x00;
+ action_stop=0x00;
+ action_running=0x00;
+ action_next_index=0;
+ // time variables (in time units (7.8 ms each time unit))
+ action_total_time=0;// fixed point 48|16 format
+ action_pre_time=0;// fixed point 48|16 format
+ action_main_time=0;// fixed point 48|16 format
+ action_step_time=0;// fixed point 48|16 format
+ action_pause_time=0;// fixed point 48|16 format
+ action_current_time=0;// fixed point 48|16 format
+ action_section_time=0;// fixed point 48|16 format
+
+ action_period=period;
+}
- // Find direction change
- if( ((wPrevTargetAngle < wCurrentTargetAngle) && (wCurrentTargetAngle < wNextTargetAngle))
- || ((wPrevTargetAngle > wCurrentTargetAngle) && (wCurrentTargetAngle > wNextTargetAngle)) )
- {
- // °è¼Ó Ãõ°¡Çðųª °¨¼ÒÇðÃ, ȤÀº °°´Ù¸é(Ãï, ºÒ¿¬¼Ó áÀÌ ¾ø´Ù¸é)
- bDirectionChanged = 0;
- }
- else
- {
- bDirectionChanged = 1;
- }
+inline void action_set_period(uint16_t period)
+{
+ action_period=period;
+}
- // Find finish type
- if( bDirectionChanged || wPauseTime || action_finished == 0x01 )
- {
- bpFinishType[bID] = ZERO_FINISH;
- }
- else
- {
- bpFinishType[bID] = NONE_ZERO_FINISH;
- }
- }
- }
- //½Ã°£À» °è»êÇؼ ´Ù½Ã 7.8msec·Î ³ª´©´Ù(<<7)-±×½Ã°£µ¿¾È¿¡ 7.8msec°¡ ¸î°³µé¾î°¡´ÂÃö °è»êÇÑ °Ã
- //´ÜÀ§ º¯È¯µÚ¿¡ °¢/¼Óµµ¸¦ ±¸ÇðÃ(½Ã°£)±× ½Ã°£¿¡ ´Ù½Ã 7.8msec°¡ ¸î°³µé¾î°¡ÀÖ´ÂÃö °è»ê
- //´ÜÀ§ º¯È¯ --- °¢ :1024°è->300µµ°è, ¼Óµµ: 256°è ->720°è
- //wUnitTimeNum = ((wMaxAngle1024*300/1024) /(wMaxSpeed256 * 720/256)) /7.8msec;
- // = ((128*wMaxAngle1024*300/1024) /(wMaxSpeed256 * 720/256)) ; (/7.8msec == *128)
- // = (wMaxAngle1024*40) /(wMaxSpeed256 *3);
- if( action_current_page.header.schedule == TIME_BASE_SCHEDULE)
- wUnitTimeTotalNum = wMaxSpeed256; //TIME BASE 051025
- else
- wUnitTimeTotalNum = (wMaxAngle1024 * 40) / (wMaxSpeed256 * 3);
+inline uint16_t action_get_period(void)
+{
+ return action_period;
+}
- wAccelStep = action_current_page.header.accel;
- if( wUnitTimeTotalNum <= (wAccelStep << 1) )
- {
- if( wUnitTimeTotalNum == 0 )
- wAccelStep = 0;
- else
- {
- wAccelStep = (wUnitTimeTotalNum - 1) >> 1;
- if( wAccelStep == 0 )
- wUnitTimeTotalNum = 0; //¿òÃ÷À̷øé Àû¾îµµ °¡¼Ó,µî¼ÓÀÌ ÇÑ ½ºÅÜÀÌ»ó¾¿Àº øÀçÇؾß
- }
- }
+uint8_t action_load_page(uint8_t page_id)
+{
+ action_next_index=page_id;
+ if(action_next_index<0 || action_next_index>255)
+ return 0x00;
+ pages_get_page(action_next_index,&action_current_page);
+ pages_get_page(action_current_page.header.next,&action_next_page);
+ if(!pages_check_checksum(&action_current_page))
+ return 0x00;
+ if(!pages_check_checksum(&action_next_page))
+ return 0x00;
+
+ return 0x01;
+}
- ulTotalTime256T = ((unsigned long)wUnitTimeTotalNum) << 1;// /128 * 256
- ulPreSectionTime256T = ((unsigned long)wAccelStep) << 1;// /128 * 256
- ulMainTime256T = ulTotalTime256T - ulPreSectionTime256T;
- lDivider1 = ulPreSectionTime256T + (ulMainTime256T << 1);
- lDivider2 = (ulMainTime256T << 1);
- if(lDivider1 == 0)
- lDivider1 = 1;
+void action_start_page(void)
+{
+ uint8_t i;
- if(lDivider2 == 0)
- lDivider2 = 1;
- for(bID=1;bID<manager_get_num_servos();bID++)
- {
- if(manager_get_index_module(bID) == MM_ACTION)
- {
- lStartSpeed1024_PreTime_256T = (long)ipLastOutSpeed1024[bID] * ulPreSectionTime256T; // *300/1024 * 1024/720 * 256 * 2
- lMovingAngle_Speed1024Scale_256T_2T = (((long)ipMovingAngle1024[bID]) * 2560L) / 12;
+ for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
+ action_start_angles[i]=action_current_angles[i];
+ action_stop=0x00;
+ action_current_time=0;
+ action_section_time=0;
+ action_current_step_index=-1;
+ ram_set_bit(DARWIN_ACTION_STATUS,0);
+ action_running=0x01;
+}
- if( bpFinishType[bID] == ZERO_FINISH )
- ipMainSpeed1024[bID] = (short int)((lMovingAngle_Speed1024Scale_256T_2T - lStartSpeed1024_PreTime_256T) / lDivider2);
- else
- ipMainSpeed1024[bID] = (short int)((lMovingAngle_Speed1024Scale_256T_2T - lStartSpeed1024_PreTime_256T) / lDivider1);
+void action_stop_page(void)
+{
+ action_stop=0x01;
+}
- if( ipMainSpeed1024[bID] > 1023 )
- ipMainSpeed1024[bID] = 1023;
+uint8_t action_is_running(void)
+{
+ return action_running;
+}
- if( ipMainSpeed1024[bID] < -1023 )
- ipMainSpeed1024[bID] = -1023;
- }
- }
- wUnitTimeNum = wAccelStep; //PreSection
+// motion manager interface
+void action_process(void)
+{
+ uint8_t i;
+ // angle variables
+ static int64_t accel_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+ static int64_t main_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format
+ // speed variables (in controller units)
+ static int64_t current_speed[PAGE_MAX_NUM_SERVOS]={0};// fixed point 48|16 format
+ int64_t delta_speed;
+ // control variables
+ static action_states state=ACTION_PAUSE;
+
+ if(action_running==0x01)
+ {
+ action_current_time+=action_period;
+ switch(state)
+ {
+ case ACTION_PRE: if(action_current_time>action_section_time)
+ {
+ for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
+ {
+ /* the last segment of the pre section */
+ delta_speed=(action_main_speed[i]-action_start_speed[i]);
+ current_speed[i]=action_start_speed[i]+delta_speed;
+ accel_angles[i]=(action_start_speed[i]*action_section_time+((delta_speed*action_section_time)>>1))>>16;
+ action_current_angles[i]=action_start_angles[i]+accel_angles[i];
+ /* update of the state */
+ if(!action_zero_speed_finish[i])
+ {
+ if((action_step_time-action_pre_time)==0)
+ main_angles[i]=0;
+ else
+ main_angles[i]=((action_moving_angles[i]-accel_angles[i])*(action_step_time-(action_pre_time<<1)))/(action_step_time-action_pre_time);
+ action_start_angles[i]=action_current_angles[i];
+ }
+ else
+ {
+ main_angles[i]=action_moving_angles[i]-accel_angles[i]-(((action_main_speed[i]*action_pre_time)>>1)>>16);
+ action_start_angles[i]=action_current_angles[i];
+ }
+ /* the first step of the main section */
+ action_current_angles[i]=action_start_angles[i]+(main_angles[i]*(action_current_time-action_section_time))/(action_step_time-(action_pre_time<<1));
+ current_speed[i]=action_main_speed[i];
+ }
+ action_current_time=action_current_time-action_section_time;
+ action_section_time=action_step_time-(action_pre_time<<1);
+ state=ACTION_MAIN;
+ }
+ else
+ {
+ for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
+ {
+ delta_speed=((action_main_speed[i]-action_start_speed[i])*action_current_time)/action_section_time;
+ current_speed[i]=action_start_speed[i]+delta_speed;
+ accel_angles[i]=((action_start_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16;
+ action_current_angles[i]=action_start_angles[i]+accel_angles[i];
+ }
+ state=ACTION_PRE;
+ }
+ break;
+ case ACTION_MAIN: if(action_current_time>action_section_time)
+ {
+ for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
+ {
+ /* last segment of the main section */
+ action_current_angles[i]=action_start_angles[i]+main_angles[i];
+ current_speed[i]=action_main_speed[i];
+ /* update state */
+ action_start_angles[i]=action_current_angles[i];
+ main_angles[i]=action_moving_angles[i]-main_angles[i]-accel_angles[i];
+ /* first segment of the post section */
+ if(action_zero_speed_finish[i])
+ {
+ delta_speed=((0.0-action_main_speed[i])*(action_current_time-action_section_time))/action_pre_time;
+ current_speed[i]=action_main_speed[i]+delta_speed;
+ action_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
+ }
+ else
+ {
+ action_current_angles[i]=action_start_angles[i]+((main_angles[i]*(action_current_time-action_section_time))/action_pre_time);
+ current_speed[i]=action_main_speed[i];
+ }
+ }
+ action_current_time=action_current_time-action_section_time;
+ action_section_time=action_pre_time;
+ state=ACTION_POST;
+ }
+ else
+ {
+ for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
+ {
+ action_current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
+ current_speed[i]=action_main_speed[i];
+ }
+ state=ACTION_MAIN;
+ }
+ break;
+ case ACTION_POST: if(action_current_time>action_section_time)
+ {
+ for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
+ {
+ /* last segment of the post section */
+ if(action_zero_speed_finish[i])
+ {
+ delta_speed=-action_main_speed[i];
+ current_speed[i]=action_main_speed[i]+delta_speed;
+ action_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_section_time)+((delta_speed*action_section_time)>>1))>>16);
+ }
+ else
+ {
+ action_current_angles[i]=action_start_angles[i]+main_angles[i];
+ current_speed[i]=action_main_speed[i];
+ }
+ /* update state */
+ action_start_angles[i]=action_current_angles[i];
+ action_start_speed[i]=current_speed[i];
+ }
+ /* load the next step */
+ if(action_pause_time==0)
+ {
+ if(action_end)
+ {
+ action_load_next_step();
+ state=ACTION_PAUSE;
+ action_end=0x00;
+ ram_clear_bit(DARWIN_ACTION_STATUS,0);
+ action_running=0x00;
+ }
+ else
+ {
+ action_load_next_step();
+ /* first step of the next section */
+ for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
+ {
+ delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time;
+ current_speed[i]=action_start_speed[i]+delta_speed;
+ accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
+ action_current_angles[i]=action_start_angles[i]+accel_angles[i];
+ }
+ action_current_time=action_current_time-action_section_time;
+ action_section_time=action_pre_time;
+ state=ACTION_PRE;
+ }
+ }
+ else
+ {
+ action_current_time=action_current_time-action_section_time;
+ action_section_time=action_pause_time;
+ state=ACTION_PAUSE;
+ }
+ }
+ else
+ {
+ for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
+ {
+ if(action_zero_speed_finish[i])
+ {
+ delta_speed=((0.0-action_main_speed[i])*action_current_time)/action_section_time;
+ current_speed[i]=action_main_speed[i]+delta_speed;
+ action_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16);
+ }
+ else
+ {
+ action_current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
+ current_speed[i]=action_main_speed[i];
+ }
+ }
+ state=ACTION_POST;
+ }
+ break;
+ case ACTION_PAUSE: if(action_current_time>action_section_time)
+ {
+ if(action_end)
+ {
+ // find next page to execute
+ action_load_next_step();
+ state=ACTION_PAUSE;
+ action_end=0x00;
+ ram_clear_bit(DARWIN_ACTION_STATUS,0);
+ action_running=0x00;
+ }
+ else
+ {
+ // find next page to execute
+ action_load_next_step();
+ /* first step of the next section */
+ for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
+ {
+ delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time;
+ current_speed[i]=action_start_speed[i]+delta_speed;
+ accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
+ action_current_angles[i]=action_start_angles[i]+accel_angles[i];
+ }
+ action_current_time=action_current_time-action_section_time;
+ action_section_time=action_pre_time;
+ state=ACTION_PRE;
+ }
+ }
+ else// pause section still active
+ state=ACTION_PAUSE;
+ break;
+ default: break;
}
}
}
+
diff --git a/src/adc.c b/src/adc.c
deleted file mode 100755
index 79d890398ef4720d798986b9038fc4db98e38496..0000000000000000000000000000000000000000
--- a/src/adc.c
+++ /dev/null
@@ -1,227 +0,0 @@
-#include "adc.h"
-#include "ram.h"
-#include "gpio.h"
-
-#define PIN_ADC0 GPIO_Pin_0
-#define PIN_ADC1 GPIO_Pin_1
-#define PIN_ADC2 GPIO_Pin_2
-#define PIN_ADC3 GPIO_Pin_3
-#define PIN_ADC4 GPIO_Pin_0
-#define PIN_ADC5 GPIO_Pin_1
-#define PIN_ADC6 GPIO_Pin_2
-#define PIN_ADC7 GPIO_Pin_3
-#define PIN_ADC8 GPIO_Pin_4
-#define PIN_ADC9 GPIO_Pin_5
-#define PIN_ADC10 GPIO_Pin_6
-#define PIN_ADC11 GPIO_Pin_7
-#define PIN_ADC12 GPIO_Pin_4
-#define PIN_ADC13 GPIO_Pin_5
-#define PIN_ADC14 GPIO_Pin_0
-#define PIN_ADC15 GPIO_Pin_1
-
-#define ADC_MAX_NUM_SAMPLES 32
-
-typedef struct {
- uint16_t samples[ADC_MAX_NUM_SAMPLES];
-}TADCChannelData;
-
-// private variables
-uint8_t adc1_current_channel;
-uint8_t adc1_current_sample;
-// ADC 1 channels
-const uint8_t adc1_channel_order[8]={ADC_Channel_0,ADC_Channel_1,ADC_Channel_2,ADC_Channel_3,ADC_Channel_10,ADC_Channel_11,ADC_Channel_12,ADC_Channel_13};
-TADCChannelData adc1_channels[8];
-// channel 0 (position 0) board voltage
-// channel 1 (position 1) microphone 1
-uint8_t adc2_current_channel;
-uint8_t adc2_current_sample;
-// ADC 2 channels
-const uint8_t adc2_channel_order[8]={ADC_Channel_4,ADC_Channel_5,ADC_Channel_6,ADC_Channel_7,ADC_Channel_8,ADC_Channel_9,ADC_Channel_14,ADC_Channel_15};
-TADCChannelData adc2_channels[8];
-// channel 9 (position 6) microphone 2
-
-// private functions
-uint8_t compute_voltage(uint16_t digital_value)
-{
- return (uint8_t)(digital_value*617/10000);// the float aoperation takes too much time in the interrupt service routine
-}
-
-// interrupt handler
-void ADC1_2_IRQHandler(void)
-{
- uint8_t voltage;
-
- if(ADC_GetITStatus(ADC1, ADC_IT_EOC) != RESET)
- {
- adc1_channels[adc1_current_channel].samples[adc1_current_sample]=ADC_GetConversionValue(ADC1);
- if(adc1_channel_order[adc1_current_channel]==ADC_Channel_0)// voltage channel
- {
- voltage=compute_voltage(adc1_channels[adc1_current_channel].samples[adc1_current_sample]);
- ram_write_byte(CM730_VOLTAGE,voltage);
- }
- else
- ram_write_word(CM730_LEFT_MIC_L+(adc1_channel_order[adc1_current_channel]<<1),adc1_channels[adc1_current_channel].samples[adc1_current_sample]);
- adc1_current_channel++;
- if(adc1_current_channel==8)
- {
- adc1_current_channel=0;
- adc1_current_sample=(adc1_current_sample+1)%ADC_MAX_NUM_SAMPLES;
- }
- ADC_ClearITPendingBit(ADC1, ADC_IT_EOC);
- }
- if(ADC_GetITStatus(ADC2, ADC_IT_EOC) != RESET)
- {
- adc2_channels[adc2_current_channel].samples[adc2_current_sample]=ADC_GetConversionValue(ADC2);
- ram_write_word(CM730_LEFT_MIC_L+(adc2_channel_order[adc2_current_channel]<<1),adc2_channels[adc2_current_channel].samples[adc2_current_sample]);
- adc2_current_channel++;
- if(adc2_current_channel==8)
- {
- adc2_current_channel=0;
- adc2_current_sample=(adc2_current_sample+1)%ADC_MAX_NUM_SAMPLES;
- }
- ADC_ClearITPendingBit(ADC2, ADC_IT_EOC);
- }
-}
-
-// public functions
-void adc_init(void)
-{
- GPIO_InitTypeDef GPIO_InitStructure;
- ADC_InitTypeDef ADC_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
- uint8_t i,j;
-
- // configure the ADC clock
- RCC_ADCCLKConfig(RCC_PCLK2_Div8);//72MHz/8
-
- // configure the pins as analog input
- GPIO_StructInit(&GPIO_InitStructure);
- GPIO_InitStructure.GPIO_Pin = PIN_ADC4 | PIN_ADC5 | PIN_ADC6 | PIN_ADC7 | PIN_ADC8 | PIN_ADC9 | PIN_ADC10 | PIN_ADC11 ;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
- GPIO_Init(GPIOA, &GPIO_InitStructure);
-
- GPIO_InitStructure.GPIO_Pin = PIN_ADC14 | PIN_ADC15 ;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
-
- GPIO_InitStructure.GPIO_Pin = PIN_ADC0 | PIN_ADC1 | PIN_ADC2 | PIN_ADC3 | PIN_ADC12 | PIN_ADC13 ;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
- GPIO_Init(GPIOC, &GPIO_InitStructure);
-
- // initialize the internal variables
- adc1_current_channel=0;
- adc1_current_sample=0;
- adc2_current_channel=0;
- adc2_current_sample=0;
- // set channels order
- for(i=0;i<8;i++)
- {
- for(j=0;j<ADC_MAX_NUM_SAMPLES;j++)
- {
- adc1_channels[i].samples[j]=0;
- adc2_channels[i].samples[j]=0;
- }
- }
-
- // configure interrupts
- NVIC_InitStructure.NVIC_IRQChannel = ADC1_2_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
- // configure the ADC module
- ADC_StructInit(&ADC_InitStructure);
- ADC_InitStructure.ADC_Mode = ADC_Mode_Independent;
- ADC_InitStructure.ADC_ScanConvMode = ENABLE;
- ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
- ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
- ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
- ADC_InitStructure.ADC_NbrOfChannel = 1;
- ADC_Init(ADC1, &ADC_InitStructure);
- ADC_Init(ADC2, &ADC_InitStructure);
-
- /* configure the first channel of ADC1 */
- for(i=0;i<8;i++)
- ADC_RegularChannelConfig(ADC1, adc1_channel_order[i], i , ADC_SampleTime_239Cycles5);
- /* enable end of conversion interrupt for ADC1 */
- ADC_ITConfig(ADC1, ADC_IT_EOC, ENABLE);
-
- /* configure the first channel of ADC2 */
- for(i=0;i<8;i++)
- ADC_RegularChannelConfig(ADC2, adc2_channel_order[i], i, ADC_SampleTime_239Cycles5);
- /* enable end of conversion interrupt for ADC2 */
- ADC_ITConfig(ADC2, ADC_IT_EOC, ENABLE);
-
- /* Enable ADC1,2 */
- ADC_Cmd(ADC1, ENABLE);
- ADC_Cmd(ADC2, ENABLE);
-
- /* Enable ADC1,2 reset calibaration register */
- /* Check the end of ADC1,2 reset calibration register */
- ADC_ResetCalibration(ADC1);
- while(ADC_GetResetCalibrationStatus(ADC1));
- ADC_ResetCalibration(ADC2);
- while(ADC_GetResetCalibrationStatus(ADC2));
-
- /* Start ADC1,2 calibaration */
- /* Check the end of ADC1,2 calibration */
- ADC_StartCalibration(ADC1);
- while(ADC_GetCalibrationStatus(ADC1));
- ADC_StartCalibration(ADC2);
- while(ADC_GetCalibrationStatus(ADC2));
-
- /* Start ADC2 Software Conversion */
- ADC_SoftwareStartConvCmd(ADC1, ENABLE);
- ADC_SoftwareStartConvCmd(ADC2, ENABLE);
-}
-
-uint16_t adc_get_channel(uint8_t channel)
-{
- uint16_t data;
-
- switch(channel)
- {
- case 0: data=adc1_channels[0].samples[adc1_current_sample];
- break;
- case 1: data=adc1_channels[1].samples[adc1_current_sample];
- break;
- case 2: data=adc1_channels[2].samples[adc1_current_sample];
- break;
- case 3: data=adc1_channels[3].samples[adc1_current_sample];
- break;
- case 4: data=adc2_channels[0].samples[adc1_current_sample];
- break;
- case 5: data=adc2_channels[1].samples[adc1_current_sample];
- break;
- case 6: data=adc2_channels[2].samples[adc1_current_sample];
- break;
- case 7: data=adc2_channels[3].samples[adc1_current_sample];
- break;
- case 8: data=adc2_channels[4].samples[adc1_current_sample];
- break;
- case 9: data=adc2_channels[5].samples[adc1_current_sample];
- break;
- case 10: data=adc1_channels[4].samples[adc1_current_sample];
- break;
- case 11: data=adc1_channels[5].samples[adc1_current_sample];
- break;
- case 12: data=adc1_channels[6].samples[adc1_current_sample];
- break;
- case 13: data=adc1_channels[7].samples[adc1_current_sample];
- break;
- case 14: data=adc2_channels[6].samples[adc1_current_sample];
- break;
- case 15: data=adc2_channels[7].samples[adc1_current_sample];
- break;
- default: data=0xFFFF;
- break;
- }
-
- return data;
-}
-
-uint16_t adc_get_averaged_channel(uint8_t channel)
-{
- return 0;
-}
diff --git a/src/adc_dma.c b/src/adc_dma.c
new file mode 100755
index 0000000000000000000000000000000000000000..0076e5f32ace44c880835fac0258df7719291350
--- /dev/null
+++ b/src/adc_dma.c
@@ -0,0 +1,228 @@
+#include "adc_dma.h"
+#include "ram.h"
+
+#define ADC1_CH1 ADC_Channel_0
+#define ADC1_CH2 ADC_Channel_1
+#define ADC1_CH3 ADC_Channel_2
+#define ADC1_CH4 ADC_Channel_3
+#define ADC1_CH5 ADC_Channel_4
+#define ADC1_CH6 ADC_Channel_5
+#define ADC1_CH7 ADC_Channel_6
+#define ADC1_CH8 ADC_Channel_7
+
+#define ADC2_CH1 ADC_Channel_8
+#define ADC2_CH2 ADC_Channel_9
+#define ADC2_CH3 ADC_Channel_10
+#define ADC2_CH4 ADC_Channel_11
+#define ADC2_CH5 ADC_Channel_12
+#define ADC2_CH6 ADC_Channel_13
+#define ADC2_CH7 ADC_Channel_14
+#define ADC2_CH8 ADC_Channel_15
+
+#define ADC1_CH1_PIN GPIO_Pin_0
+#define ADC1_CH1_PORT GPIOA
+#define ADC1_CH1_PORT_CLK RCC_APB2Periph_GPIOA
+#define ADC1_CH2_PIN GPIO_Pin_1
+#define ADC1_CH2_PORT GPIOA
+#define ADC1_CH2_PORT_CLK RCC_APB2Periph_GPIOA
+#define ADC1_CH3_PIN GPIO_Pin_2
+#define ADC1_CH3_PORT GPIOA
+#define ADC1_CH3_PORT_CLK RCC_APB2Periph_GPIOA
+#define ADC1_CH4_PIN GPIO_Pin_3
+#define ADC1_CH4_PORT GPIOA
+#define ADC1_CH4_PORT_CLK RCC_APB2Periph_GPIOA
+#define ADC1_CH5_PIN GPIO_Pin_4
+#define ADC1_CH5_PORT GPIOA
+#define ADC1_CH5_PORT_CLK RCC_APB2Periph_GPIOA
+#define ADC1_CH6_PIN GPIO_Pin_5
+#define ADC1_CH6_PORT GPIOA
+#define ADC1_CH6_PORT_CLK RCC_APB2Periph_GPIOA
+#define ADC1_CH7_PIN GPIO_Pin_6
+#define ADC1_CH7_PORT GPIOA
+#define ADC1_CH7_PORT_CLK RCC_APB2Periph_GPIOA
+#define ADC1_CH8_PIN GPIO_Pin_7
+#define ADC1_CH8_PORT GPIOA
+#define ADC1_CH8_PORT_CLK RCC_APB2Periph_GPIOA
+
+#define ADC2_CH1_PIN GPIO_Pin_0
+#define ADC2_CH1_PORT GPIOB
+#define ADC2_CH1_PORT_CLK RCC_APB2Periph_GPIOB
+#define ADC2_CH2_PIN GPIO_Pin_1
+#define ADC2_CH2_PORT GPIOB
+#define ADC2_CH2_PORT_CLK RCC_APB2Periph_GPIOB
+#define ADC2_CH3_PIN GPIO_Pin_0
+#define ADC2_CH3_PORT GPIOC
+#define ADC2_CH3_PORT_CLK RCC_APB2Periph_GPIOC
+#define ADC2_CH4_PIN GPIO_Pin_1
+#define ADC2_CH4_PORT GPIOC
+#define ADC2_CH4_PORT_CLK RCC_APB2Periph_GPIOC
+#define ADC2_CH5_PIN GPIO_Pin_2
+#define ADC2_CH5_PORT GPIOC
+#define ADC2_CH5_PORT_CLK RCC_APB2Periph_GPIOC
+#define ADC2_CH6_PIN GPIO_Pin_3
+#define ADC2_CH6_PORT GPIOC
+#define ADC2_CH6_PORT_CLK RCC_APB2Periph_GPIOC
+#define ADC2_CH7_PIN GPIO_Pin_4
+#define ADC2_CH7_PORT GPIOC
+#define ADC2_CH7_PORT_CLK RCC_APB2Periph_GPIOC
+#define ADC2_CH8_PIN GPIO_Pin_5
+#define ADC2_CH8_PORT GPIOC
+#define ADC2_CH8_PORT_CLK RCC_APB2Periph_GPIOC
+
+#define ADC_DMA_CHANNEL DMA1_Channel1
+#define ADC_DMA_FLAG_GLIF DMA1_FLAG_GL1
+#define ADC_DMA_FLAG_TEIF DMA1_FLAG_TE1
+#define ADC_DMA_FLAG_HTIF DMA1_FLAG_HT1
+#define ADC_DMA_FLAG_TCIF DMA1_FLAG_TC1
+
+#define ADC_DMA_IRQn DMA1_Channel1_IRQn
+#define ADC_DMA_IRQHandler DMA1_Channel1_IRQHandler
+
+#define ADC_CCR_ADDRESS ((uint32_t)0x4001244C)
+
+// private variables
+
+// interrupt handlers
+
+// public functions
+void adc_init(void)
+{
+ ADC_InitTypeDef ADC_InitStructure;
+ GPIO_InitTypeDef GPIO_InitStructure;
+ DMA_InitTypeDef DMA_InitStructure;
+
+ //==Configure the systems clocks for the ADC and DMA==
+ //ADCCLK = PCLK2 / 4
+ RCC_ADCCLKConfig(RCC_PCLK2_Div8);
+
+ /* enable clocks */
+ RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC2, ENABLE);
+ RCC_APB2PeriphClockCmd(ADC1_CH1_PORT_CLK | ADC1_CH2_PORT_CLK | ADC1_CH3_PORT_CLK | ADC1_CH4_PORT_CLK |
+ ADC1_CH5_PORT_CLK | ADC1_CH6_PORT_CLK | ADC1_CH7_PORT_CLK | ADC1_CH8_PORT_CLK, ENABLE);
+ RCC_APB2PeriphClockCmd(ADC2_CH1_PORT_CLK | ADC2_CH2_PORT_CLK | ADC2_CH3_PORT_CLK | ADC2_CH4_PORT_CLK |
+ ADC2_CH1_PORT_CLK | ADC2_CH2_PORT_CLK | ADC2_CH3_PORT_CLK | ADC2_CH4_PORT_CLK, ENABLE);
+
+ /* DMA Config */
+ DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&ram_data[DARWIN_MIC1_L];
+ DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)ADC_CCR_ADDRESS;
+ DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
+ DMA_InitStructure.DMA_BufferSize = 32;
+ DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+ DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+ DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
+ DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+ DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
+ DMA_InitStructure.DMA_Priority = DMA_Priority_High;
+ DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
+ DMA_Init(ADC_DMA_CHANNEL,&DMA_InitStructure);
+
+ /* DMA2_Stream0 enable */
+ DMA_Cmd(ADC_DMA_CHANNEL, ENABLE);
+
+ /* configure GPIO */
+ GPIO_InitStructure.GPIO_Pin = ADC1_CH1_PIN;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(ADC1_CH1_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = ADC1_CH2_PIN;
+ GPIO_Init(ADC1_CH2_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = ADC1_CH3_PIN;
+ GPIO_Init(ADC1_CH3_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = ADC1_CH4_PIN;
+ GPIO_Init(ADC1_CH4_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = ADC1_CH5_PIN;
+ GPIO_Init(ADC1_CH5_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = ADC1_CH6_PIN;
+ GPIO_Init(ADC1_CH6_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = ADC1_CH7_PIN;
+ GPIO_Init(ADC1_CH7_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = ADC1_CH8_PIN;
+ GPIO_Init(ADC1_CH8_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = ADC2_CH1_PIN;
+ GPIO_Init(ADC2_CH1_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = ADC2_CH2_PIN;
+ GPIO_Init(ADC2_CH2_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = ADC2_CH3_PIN;
+ GPIO_Init(ADC2_CH3_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = ADC2_CH4_PIN;
+ GPIO_Init(ADC2_CH4_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = ADC2_CH5_PIN;
+ GPIO_Init(ADC2_CH5_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = ADC2_CH6_PIN;
+ GPIO_Init(ADC2_CH6_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = ADC2_CH7_PIN;
+ GPIO_Init(ADC2_CH7_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = ADC2_CH8_PIN;
+ GPIO_Init(ADC2_CH8_PORT, &GPIO_InitStructure);
+
+ /* ADC1 Common Init */
+ ADC_InitStructure.ADC_Mode = ADC_Mode_RegSimult;
+ ADC_InitStructure.ADC_ScanConvMode = ENABLE;
+ ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
+ ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
+ ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
+ ADC_InitStructure.ADC_NbrOfChannel = 8;
+ ADC_Init(ADC1, &ADC_InitStructure);
+
+ /* ADC1 regular channels configuration */
+ ADC_RegularChannelConfig(ADC1, ADC1_CH1, 1, ADC_SampleTime_239Cycles5);
+ ADC_RegularChannelConfig(ADC1, ADC1_CH2, 2, ADC_SampleTime_239Cycles5);
+ ADC_RegularChannelConfig(ADC1, ADC1_CH3, 3, ADC_SampleTime_239Cycles5);
+ ADC_RegularChannelConfig(ADC1, ADC1_CH4, 4, ADC_SampleTime_239Cycles5);
+ ADC_RegularChannelConfig(ADC1, ADC1_CH5, 5, ADC_SampleTime_239Cycles5);
+ ADC_RegularChannelConfig(ADC1, ADC1_CH6, 6, ADC_SampleTime_239Cycles5);
+ ADC_RegularChannelConfig(ADC1, ADC1_CH7, 7, ADC_SampleTime_239Cycles5);
+ ADC_RegularChannelConfig(ADC1, ADC1_CH8, 8, ADC_SampleTime_239Cycles5);
+ /* Enable ADC1 DMA */
+ ADC_DMACmd(ADC1, ENABLE);
+
+ /* ADC2 configuration */
+ ADC_InitStructure.ADC_Mode = ADC_Mode_RegSimult;
+ ADC_InitStructure.ADC_ScanConvMode = ENABLE;
+ ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
+ ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
+ ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
+ ADC_InitStructure.ADC_NbrOfChannel = 8;
+ ADC_Init(ADC2, &ADC_InitStructure);
+
+ /* ADC1 regular channels 10, 11 configuration */
+ ADC_RegularChannelConfig(ADC2, ADC2_CH1, 1, ADC_SampleTime_239Cycles5);
+ ADC_RegularChannelConfig(ADC2, ADC2_CH2, 2, ADC_SampleTime_239Cycles5);
+ ADC_RegularChannelConfig(ADC2, ADC2_CH3, 3, ADC_SampleTime_239Cycles5);
+ ADC_RegularChannelConfig(ADC2, ADC2_CH4, 4, ADC_SampleTime_239Cycles5);
+ ADC_RegularChannelConfig(ADC2, ADC2_CH5, 5, ADC_SampleTime_239Cycles5);
+ ADC_RegularChannelConfig(ADC2, ADC2_CH6, 6, ADC_SampleTime_239Cycles5);
+ ADC_RegularChannelConfig(ADC2, ADC2_CH7, 7, ADC_SampleTime_239Cycles5);
+ ADC_RegularChannelConfig(ADC2, ADC2_CH8, 8, ADC_SampleTime_239Cycles5);
+ /* Enable ADC2 external trigger conversion */
+ ADC_ExternalTrigConvCmd(ADC2, ENABLE);
+ /* Enable ADC1 DMA */
+ ADC_DMACmd(ADC2, ENABLE);
+
+ /* Enable ADC1 */
+ ADC_Cmd(ADC1, ENABLE);
+ /* Enable ADC1 reset calibration register */
+ ADC_ResetCalibration(ADC1);
+ /* Check the end of ADC1 reset calibration register */
+ while(ADC_GetResetCalibrationStatus(ADC1));
+ /* Start ADC1 calibration */
+ ADC_StartCalibration(ADC1);
+ /* Check the end of ADC1 calibration */
+ while(ADC_GetCalibrationStatus(ADC1));
+
+ /* Enable ADC2 */
+ ADC_Cmd(ADC2, ENABLE);
+ /* Enable ADC2 reset calibration register */
+ ADC_ResetCalibration(ADC2);
+ /* Check the end of ADC2 reset calibration register */
+ while(ADC_GetResetCalibrationStatus(ADC2));
+ /* Start ADC2 calibration */
+ ADC_StartCalibration(ADC2);
+ /* Check the end of ADC2 calibration */
+ while(ADC_GetCalibrationStatus(ADC2));
+
+ /* Start ADC1 Software Conversion */
+ ADC_SoftwareStartConvCmd(ADC1, ENABLE);
+}
diff --git a/src/cm730_fw.c b/src/cm730_fw.c
index f973378e42ff2cc9ef1461e049e822d1abebf436..e233c0dbf3c590b6cd628360c05dcebcf98db931 100755
--- a/src/cm730_fw.c
+++ b/src/cm730_fw.c
@@ -1,342 +1,187 @@
-/**
- ******************************************************************************
- * @file Project/STM32F10x_StdPeriph_Template/main.c
- * @author MCD Application Team
- * @version V3.5.0
- * @date 08-April-2011
- * @brief Main program body
- ******************************************************************************
- * @attention
- *
- * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
- * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
- * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
- * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
- * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
- * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
- *
- * <h2><center>© COPYRIGHT 2011 STMicroelectronics</center></h2>
- ******************************************************************************
- */
-
-/* Includes ------------------------------------------------------------------*/
-#include <math.h>
#include "stm32f10x.h"
-#include "system_init.h"
-#include "dynamixel_slave.h"
-#include "dynamixel_master.h"
-#include "motion_manager.h"
-#include "joint_motion.h"
-#include "walking.h"
-#include "dyn_servos.h"
-#include "action.h"
-#include "motion_pages.h"
#include "gpio.h"
#include "time.h"
-#include "utils.h"
#include "eeprom.h"
+#include "motion_pages.h"
+#include "motion_manager.h"
+#include "action.h"
+#include "walking.h"
+#include "dynamixel_slave_uart_dma.h"
+#include "dynamixel_master_uart_dma.h"
#include "ram.h"
-#include "adc.h"
#include "imu.h"
+#include "adc_dma.h"
uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data)
{
- if(address >= CM730_MODEL_NUMBER_L && address < EEPROM_DATA_SIZE) // EEPROM region
- {
- if(eeprom_read_table(address,length,data)==EEPROM_SUCCESS)
- return DYN_NO_ERROR;
- else
- return DYN_INST_ERROR;
- }
- else// RAM region
- {
- if(ram_read_table(address,length,data)==RAM_SUCCESS)
- return DYN_NO_ERROR;
- else
- return DYN_INST_ERROR;
- }
+ uint8_t error;
+
+ // pre-read operation
+ // read operation
+ error=ram_read_table(address,length,data);
+ // post-read operation
+ if(ram_in_range(DARWIN_PUSHBUTTON,DARWIN_PUSHBUTTON,address,length))
+ ram_data[DARWIN_PUSHBUTTON]=0x00;
+
+ return error;
}
-uint8_t write_operation(uint8_t address, uint8_t length, uint8_t *data)
+uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
{
- uint8_t error=DYN_NO_ERROR,i=0,j=0,remaining_length;
- // joint motion variables
- int16_t joint_angles[JOINT_MAX_NUM_SERVOS];
- uint8_t joint_variable[JOINT_MAX_NUM_SERVOS];
- uint8_t first,joint_num;
- // led variables
- uint8_t R=0x00,G=0x00,B=0x00;
+ uint8_t error,byte_value,i,j,do_op=0x00;
+ uint16_t word_value;
- remaining_length=length;
- if(address >= CM730_MODEL_NUMBER_L && address < EEPROM_DATA_SIZE) // EEPROM region
+ // pre-write operation
+ if(ram_in_range(DARWIN_DXL_POWER,DARWIN_DXL_POWER,address,length))
{
- if(eeprom_write_table(address,length,data)==EEPROM_SUCCESS)
- return DYN_NO_ERROR;
+ if(data[DARWIN_DXL_POWER-address]&0x01) dyn_master_enable_power();
+ else dyn_master_disable_power();
+ }
+ if(ram_in_range(DARWIN_LED_PANNEL,DARWIN_LED_PANNEL,address,length))
+ {
+ byte_value=data[DARWIN_LED_PANNEL-address];
+ if(byte_value&0x01)
+ gpio_set_led(LED_2);
+ else
+ gpio_clear_led(LED_2);
+ if(byte_value&0x02)
+ gpio_set_led(LED_3);
else
- return DYN_INST_ERROR;
+ gpio_clear_led(LED_3);
+ if(byte_value&0x04)
+ gpio_set_led(LED_4);
+ else
+ gpio_clear_led(LED_4);
+ if(byte_value&0x08)
+ gpio_set_led(LED_TX);
+ else
+ gpio_clear_led(LED_TX);
+ if(byte_value&0x10)
+ gpio_set_led(LED_RX);
+ else
+ gpio_clear_led(LED_RX);
}
- else// RAM region
+ if(ram_in_range(DARWIN_MM_PERIOD,DARWIN_MM_PERIOD+1,address,length))
{
- while(remaining_length>0)
+ word_value=data[DARWIN_MM_PERIOD-address]+(data[DARWIN_MM_PERIOD+1-address]<<8);
+ action_set_period(word_value);
+ word_value=(word_value*1000000)>>16;
+ manager_set_period(word_value);
+ }
+ for(i=DARWIN_MM_MODULE_EN0,j=0;i<=DARWIN_MM_MODULE_EN15;i++,j+=2)
+ {
+ do_op=0x01;
+ if(ram_in_range(i,i,address,length))
{
- if(address==CM730_DXL_POWER)
- {
- if(data[i]==0x00)
- dyn_master_power_off();
- else
- dyn_master_power_on();
- i++;
- address++;
- remaining_length--;
- }
- else if(address==CM730_LED_PANNEL)
- {
- if(data[i]&0x01)
- set_led2();
- else
- clear_led2();
- if(data[i]&0x02)
- set_led3();
- else
- clear_led3();
- if(data[i]&0x04)
- set_led4();
- else
- clear_led4();
- i++;
- address++;
- remaining_length--;
- }
- else if(address==CM730_LED_HEAD_L)
- {
- if(remaining_length>1)
- {
- R=data[i]&0x1F;
- G=(data[i]&0x70)>>5;
- i++;
- remaining_length--;
- G|=(data[i]&0x03)<<3;
- B=(data[i]&0x7C)>>3;
- set_head_color(R,G,B);
- i++;
- address++;
- remaining_length--;
- }
- else
- {
- remaining_length=0;
- error=DYN_INST_ERROR;
- }
- }
- else if(address==CM730_LED_EYE_L)
- {
- if(remaining_length>1)
- {
- R=data[i]&0x1F;
- G=(data[i]&0x70)>>5;
- i++;
- remaining_length--;
- G|=(data[i]&0x03)<<3;
- B=(data[i]&0x7C)>>3;
- set_eyes_color(R,G,B);
- i++;
- address++;
- remaining_length--;
- }
- else
- {
- remaining_length=0;
- error=DYN_INST_ERROR;
- }
- }
- else if(address==CM730_ACTION_NUMBER)
- {
- action_set_page(data[i]);
- i++;
- address++;
- remaining_length--;
- }
- else if(address==CM730_ACTION_CONTROL)
- {
- if(data[i]&0x01)
- action_start_page();
- else if(data[i]&0x02)
- action_stop_page();
- i++;
- address++;
- remaining_length--;
- }
- else if(address==CM730_WALKING_CONTROL)
- {
- if(data[i]&0x01)
- walking_start();
- else if(data[i]&0x02)
- walking_stop();
- if(data[i]&0x08)
- walking_enable_autobalance();
- else
- walking_disable_autobalance();
- if(data[i]&0x10)
- walking_enable_aim();
- else
- walking_disable_aim();
- i++;
- address++;
- remaining_length--;
- }
- else if(address==CM730_JOINT_CONTROL)
- {
- if(data[i]&0x01)
- joint_motion_start();
- else if(data[i]&0x02)
- joint_motion_stop();
- i++;
- address++;
- remaining_length--;
- }
- else if(address>=CM730_X_OFFSET && address<=CM730_I_GAIN)
- {
- i++;
- address++;
- remaining_length--;
- }
- else if(address>=CM730_SERVO_1_POS_L && address<=CM730_SERVO_30_POS_L)
- {
- first=address-CM730_SERVO_1_POS_L;
- if((first%2)==1)
- {
- error=DYN_INST_ERROR;
- remaining_length=0;
- }
- else
- {
- first/=2;
- if((address+remaining_length)>CM730_SERVO_30_POS_H)
- joint_num=((CM730_SERVO_30_POS_L-address)/2)+1;
- else
- joint_num=remaining_length/2;
- for(j=0;j<joint_num;j++,i+=2,remaining_length-=2,address+=2)
- joint_angles[j]=data[i]+data[i+1]*256;
- joint_motion_load_angles(first,joint_num,joint_angles);
- }
- }
- else if(address>=CM730_SERVO_1_SPEED && address<=CM730_SERVO_30_SPEED)
- {
- first=address-CM730_SERVO_1_SPEED;
- if((address+remaining_length)>CM730_SERVO_30_SPEED)
- joint_num=CM730_SERVO_30_SPEED-address;
- else
- joint_num=remaining_length;
- for(j=0;j<joint_num;j++,i++,remaining_length--,address++)
- joint_variable[j]=data[i];
- joint_motion_load_speeds(first,joint_num,joint_variable);
- }
- else if(address>=CM730_SERVO_1_ACCEL && address<=CM730_SERVO_30_ACCEL)
- {
- first=(address-CM730_SERVO_1_ACCEL);
- if((address+remaining_length)>CM730_SERVO_30_ACCEL)
- joint_num=CM730_SERVO_30_SPEED-address;
- else
- joint_num=remaining_length;
- for(j=0;j<joint_num;j++,i++,remaining_length--,address++)
- joint_variable[j]=data[i];
- joint_motion_load_accels(first,joint_num,joint_variable);
- }
- else if(address>=CM730_MODULE_SEL_1 && address<=CM730_MODULE_SEL_8)
- {
- manager_set_module((address-CM730_MODULE_SEL_1)*4,data[i]&0x03);
- manager_set_module((address-CM730_MODULE_SEL_1)*4+1,(data[i]>>2)&0x03);
- manager_set_module((address-CM730_MODULE_SEL_1)*4+2,(data[i]>>4)&0x03);
- manager_set_module((address-CM730_MODULE_SEL_1)*4+3,(data[i]>>6)&0x03);
- i++;
- address++;
- remaining_length--;
- }
- else
- error=DYN_INST_ERROR;
+ byte_value=data[i-address];
+ if(byte_value&0x80) manager_enable_servo(j);
+ else manager_disable_servo(j);
+ manager_set_module(j,(byte_value&0x70)>>4);
+ if(byte_value&0x08) manager_enable_servo(j+1);
+ else manager_disable_servo(j+1);
+ manager_set_module(j+1,byte_value&0x07);
}
- return error;
}
-}
-
-void process_packet(uint8_t *packet)
-{
- uint8_t data[MAX_DATA_LENGTH];
- uint8_t error,length,address;
-
- switch(dyn_get_instruction(packet))
+ if(do_op)
{
- case DYN_PING: dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
- break;
- case DYN_READ: error=read_operation(dyn_get_read_address(packet),dyn_get_read_length(packet),data);
- if(error==DYN_NO_ERROR)
- dyn_slave_send_status_packet(error,dyn_get_read_length(packet),data);
- else
- dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
- break;
- case DYN_WRITE: length=dyn_get_write_data(packet,data);
- address=dyn_get_write_address(packet);
- error=write_operation(address,length,data);
- dyn_slave_send_status_packet(error,0,data);
- // update the device address
- if(address<=CM730_ID && (address+length)>=CM730_ID)
- dyn_slave_set_address(eeprom_read_device_id());
- // initialize the dynamixel master
- if(address<=CM730_DXL_POWER && (address+length)>=CM730_DXL_POWER)
- {
- if(data[address-CM730_DXL_POWER]==0x01)
- {
- delay_ms(1000);
- dyn_master_flush();
- // read the current position of the servos
- manager_read_current_position();
- // enable motion manager
- manager_enable();
- }
- else
- manager_disable();
- }
- break;
- case DYN_REG_WRITE:
- break;
- case DYN_ACTION:
- break;
- case DYN_RESET:
- break;
- case DYN_SYNC_WRITE:
- break;
+ do_op=0x00;
+ manager_disable_servos();
+ }
+ if(ram_in_range(DARWIN_MM_CONTROL,DARWIN_MM_CONTROL,address,length))
+ {
+ if(data[DARWIN_MM_CONTROL-address]&0x01)
+ manager_enable();
+ else
+ manager_disable();
+ }
+ if(ram_in_range(DARWIN_ACTION_CONTROL,DARWIN_ACTION_CONTROL,address,length))
+ {
+ if(data[DARWIN_ACTION_CONTROL-address]&0x01)
+ action_start_page();
+ if(data[DARWIN_ACTION_CONTROL-address]&0x02)
+ action_stop_page();
+ }
+ if(ram_in_range(DARWIN_ACTION_PAGE,DARWIN_ACTION_PAGE,address,length))// load the page identifier
+ action_load_page(data[DARWIN_ACTION_PAGE-address]);
+ if(ram_in_range(DARWIN_WALK_CONTROL,DARWIN_WALK_CONTROL,address,length))
+ {
+ if(data[DARWIN_WALK_CONTROL-address]&0x01)
+ walking_start();
+ if(data[DARWIN_WALK_CONTROL-address]&0x02)
+ walking_stop();
+ }
+ if(ram_in_range(DARWIN_IMU_CONTROL,DARWIN_IMU_CONTROL,address,length))// load the page identifier
+ {
+ if(data[DARWIN_IMU_CONTROL-address]&0x01)
+ imu_start();
+ else
+ imu_stop();
}
+ // write eeprom
+ for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
+ EE_WriteVariable(i,data[j]);
+ // write operation
+ error=ram_write_table(address,length,data);
+ // post-write operation
+
+ return error;
}
int main(void)
{
uint8_t inst_packet[1024];
uint8_t status_packet[1024];
+ uint8_t data[1024],error,length;
+ uint16_t eeprom_data;
+ uint16_t action_period,mm_period;
+ uint8_t i;
- // initialize the general features of the system
- clocks_init();
- // initialize the internal RAM of the device
+ /* initialize EEPROM */
+ EE_Init();
+ // initialize the Dynamixel RAM memory space
ram_init();
- // initialize the GPIO peripheral
- gpio_init();
- // initialize the time base module
+ /* initialize the 1ms system timer */
time_init();
- // initialize the ADC module
- adc_init();
- // initialize the IMU module
- imu_init();
- // initialize the dynamixel slave module
- dyn_slave_init();
- dyn_slave_set_address(eeprom_read_device_id());
- // initialize the dynamixel master module
+ /* initialize the gpio */
+ gpio_init();
+ /* initialize the dynamixel master interface */
dyn_master_init();
- dyn_master_set_timeout(300);
- // initialize the motion manager module
- manager_init();
-
- set_led2();
+ dyn_master_set_timeout(50);
+ /* initialize the dynamixel slave interface*/
+ dyn_slave_init();
+ EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data);
+ dyn_slave_set_address((uint8_t)eeprom_data);
+ EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data);
+ dyn_slave_set_return_delay((uint8_t)eeprom_data);
+ EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data);
+ dyn_slave_set_return_level((uint8_t)eeprom_data);
+ // initialize motion manager
+ EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data);
+ action_period=eeprom_data&0x00FF;
+ EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data);
+ action_period+=((eeprom_data&0x00FF)<<8);
+ mm_period=(action_period*1000000)>>16;
+ manager_init(mm_period);
+ // initialize the action module
+ action_init(action_period);
+ walking_init();
+ // initialize imu
+ imu_init();
+ // initialize adc
+ adc_init();
+
+ gpio_blink_led(LED_2,1000);
- while(1)
+ for(i=0;i<20;i++)
+ {
+ manager_enable_servo(i);
+ manager_set_module(i,MM_WALKING);
+ }
+// manager_enable();
+
+ walking_start();
+
+ while(1) /* main function does not return */
{
if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received
{
@@ -345,26 +190,47 @@ int main(void)
if(dyn_check_checksum(inst_packet)==0xFF)// the incomming packet is okay
{
// check address
- if(dyn_get_id(inst_packet)==DYN_BROADCAST_ID)// broadcast id
- {
- // send the incomming packet to the dynamixel bus
- dyn_master_resend_inst_packet(inst_packet,status_packet);
- // process the packet
- process_packet(inst_packet);
- }
- else if(dyn_get_id(inst_packet)==dyn_slave_get_address())// the packet is addressed to this device
+ if(dyn_get_id(inst_packet)==dyn_slave_get_address())// the packet is addressed to this device
{
// process the packet
- process_packet(inst_packet);
+ switch(dyn_get_instruction(inst_packet))
+ {
+ case DYN_PING: dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
+ break;
+ case DYN_READ: error=read_operation(dyn_get_read_address(inst_packet),dyn_get_read_length(inst_packet),data);
+ if(dyn_slave_get_return_level()!=0)
+ {
+ if(error==RAM_SUCCESS)
+ dyn_slave_send_status_packet(DYN_NO_ERROR,dyn_get_read_length(inst_packet),data);
+ else
+ dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
+ }
+ break;
+ case DYN_WRITE: length=dyn_get_write_data(inst_packet,data);
+ if(length!=DYN_BAD_FORMAT)
+ error=write_operation(dyn_get_write_address(inst_packet),length,data);
+ if(dyn_slave_get_return_level()==2)
+ {
+ if(error==RAM_SUCCESS || length==DYN_BAD_FORMAT)
+ dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
+ else
+ dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
+ }
+ break;
+ default:
+ break;
+ }
}
else// the packet is addressed to another device
- {
+ {
// send the incomming packet to the dynamixel bus
if(dyn_master_resend_inst_packet(inst_packet,status_packet)==DYN_NO_ERROR)
{
// send the answer back to the computer
dyn_slave_resend_status_packet(status_packet);
}
+ else
+ dyn_slave_reset();// prepare the dynamixel master to accept data
}
}
else
diff --git a/src/darwin_kinematics.c b/src/darwin_kinematics.c
new file mode 100755
index 0000000000000000000000000000000000000000..a7791fb1931fc7c328288863e0e5fdaffab7c2eb
--- /dev/null
+++ b/src/darwin_kinematics.c
@@ -0,0 +1,87 @@
+#include "darwin_kinematics.h"
+#include "darwin_math.h"
+
+const float LEG_SIDE_OFFSET = 37.0; //mm
+const float THIGH_LENGTH = 93.0; //mm
+const float CALF_LENGTH = 93.0; //mm
+const float ANKLE_LENGTH = 33.5; //mm
+const float LEG_LENGTH = 219.5; //mm (THIGH_LENGTH + CALF_LENGTH + ANKLE_LENGTH)
+
+uint8_t computeIK(float *out, float x, float y, float z, float a, float b, float c)
+{
+ TMatrix3D Tad, Tda, Tcd, Tdc, Tac;
+ TVector3D vec,orientation;
+ TPoint3D position;
+ float _Rac, _Acos, _Atan, _k, _l, _m, _n, _s, _c, _theta;
+
+ vector3d_load(&orientation,a * 180.0 / PI, b * 180.0 / PI, c * 180.0 / PI);
+ point3d_load(&position,x, y, z - LEG_LENGTH);
+ matrix3d_set_transform(&Tad,&position,&orientation);
+
+ vec.x = x + Tad.coef[2] * ANKLE_LENGTH;
+ vec.y = y + Tad.coef[6] * ANKLE_LENGTH;
+ vec.z = (z - LEG_LENGTH) + Tad.coef[10] * ANKLE_LENGTH;
+
+ // Get Knee
+ _Rac = vector3d_length(&vec);
+ _Acos = acos((_Rac * _Rac - THIGH_LENGTH * THIGH_LENGTH - CALF_LENGTH * CALF_LENGTH) / (2 * THIGH_LENGTH * CALF_LENGTH));
+ if(isnan(_Acos) == 1)
+ return 0x00;;
+ *(out + 3) = _Acos;
+
+ // Get Ankle Roll
+ Tda = Tad;
+ if(matrix3d_inverse(&Tda,&Tda) == 0x00)
+ return 0x00;
+ _k = sqrt(Tda.coef[7] * Tda.coef[7] + Tda.coef[11] * Tda.coef[11]);
+ _l = sqrt(Tda.coef[7] * Tda.coef[7] + (Tda.coef[11] - ANKLE_LENGTH) * (Tda.coef[11] - ANKLE_LENGTH));
+ _m = (_k * _k - _l * _l - ANKLE_LENGTH * ANKLE_LENGTH) / (2 * _l * ANKLE_LENGTH);
+ if(_m > 1.0)
+ _m = 1.0;
+ else if(_m < -1.0)
+ _m = -1.0;
+ _Acos = acos(_m);
+ if(isnan(_Acos) == 1)
+ return 0x00;
+ if(Tda.coef[7] < 0.0)
+ *(out + 5) = -_Acos;
+ else
+ *(out + 5) = _Acos;
+ // Get Hip Yaw
+ vector3d_load(&orientation,*(out + 5) * 180.0 / PI, 0, 0);
+ point3d_load(&position,0, 0, -ANKLE_LENGTH);
+ matrix3d_set_transform(&Tcd,&position,&orientation);
+ Tdc = Tcd;
+ if(matrix3d_inverse(&Tdc,&Tdc) == 0x00)
+ return 0x00;
+ matrix3d_mult(&Tac,&Tad,&Tdc);
+ _Atan = atan2(-Tac.coef[1] , Tac.coef[5]);
+ if(isinf(_Atan) == 1)
+ return 0x00;
+ *(out) = _Atan;
+
+ // Get Hip Roll
+ _Atan = atan2(Tac.coef[9], -Tac.coef[1] * sin(*(out)) + Tac.coef[5] * cos(*(out)));
+ if(isinf(_Atan) == 1)
+ return 0x00;
+ *(out + 1) = _Atan;
+
+ // Get Hip Pitch and Ankle Pitch
+ _Atan = atan2(Tac.coef[2] * cos(*(out)) + Tac.coef[6] * sin(*(out)), Tac.coef[0] * cos(*(out)) + Tac.coef[4] * sin(*(out)));
+ if(isinf(_Atan) == 1)
+ return 0x00;
+ _theta = _Atan;
+ _k = sin(*(out + 3)) * CALF_LENGTH;
+ _l = -THIGH_LENGTH - cos(*(out + 3)) * CALF_LENGTH;
+ _m = cos(*(out)) * vec.x + sin(*(out)) * vec.y;
+ _n = cos(*(out + 1)) * vec.z + sin(*(out)) * sin(*(out + 1)) * vec.x - cos(*(out)) * sin(*(out + 1)) * vec.y;
+ _s = (_k * _n + _l * _m) / (_k * _k + _l * _l);
+ _c = (_n - _k * _s) / _l;
+ _Atan = atan2(_s, _c);
+ if(isinf(_Atan) == 1)
+ return 0x00;
+ *(out + 2) = _Atan;
+ *(out + 4) = _theta - *(out + 3) - *(out + 2);
+
+ return 0x01;
+}
diff --git a/src/matrix.c b/src/darwin_math.c
similarity index 83%
rename from src/matrix.c
rename to src/darwin_math.c
index 20c01b1b93df79524c14bfc3ea865081f221a960..5a4b80cd4df6e3afd6c6ac3e9835c491c5abc0d4 100755
--- a/src/matrix.c
+++ b/src/darwin_math.c
@@ -1,167 +1,238 @@
-#include <math.h>
-#include "matrix.h"
-
-// public functions
-void matrix3d_init(TMatrix3D *matrix)
-{
- matrix3d_identity(matrix);
-}
-
-void matrix3d_copy(TMatrix3D *matrix_dst, TMatrix3D *matrix_src)
-{
- matrix_dst->coef[m00]=matrix_src->coef[m00];
- matrix_dst->coef[m01]=matrix_src->coef[m01];
- matrix_dst->coef[m02]=matrix_src->coef[m02];
- matrix_dst->coef[m03]=matrix_src->coef[m03];
- matrix_dst->coef[m10]=matrix_src->coef[m10];
- matrix_dst->coef[m11]=matrix_src->coef[m11];
- matrix_dst->coef[m12]=matrix_src->coef[m12];
- matrix_dst->coef[m13]=matrix_src->coef[m13];
- matrix_dst->coef[m20]=matrix_src->coef[m20];
- matrix_dst->coef[m21]=matrix_src->coef[m21];
- matrix_dst->coef[m22]=matrix_src->coef[m22];
- matrix_dst->coef[m23]=matrix_src->coef[m23];
- matrix_dst->coef[m30]=matrix_src->coef[m30];
- matrix_dst->coef[m31]=matrix_src->coef[m31];
- matrix_dst->coef[m32]=matrix_src->coef[m32];
- matrix_dst->coef[m33]=matrix_src->coef[m33];
-}
-
-void matrix3d_identity(TMatrix3D *matrix)
-{
- matrix->coef[m00]=1.0;
- matrix->coef[m01]=0.0;
- matrix->coef[m02]=0.0;
- matrix->coef[m03]=0.0;
- matrix->coef[m10]=0.0;
- matrix->coef[m11]=1.0;
- matrix->coef[m12]=0.0;
- matrix->coef[m13]=0.0;
- matrix->coef[m20]=0.0;
- matrix->coef[m21]=0.0;
- matrix->coef[m22]=1.0;
- matrix->coef[m23]=0.0;
- matrix->coef[m30]=0.0;
- matrix->coef[m31]=0.0;
- matrix->coef[m32]=0.0;
- matrix->coef[m33]=1.0;
-}
-
-uint8_t matrix3d_inverse(TMatrix3D *org, TMatrix3D *inv)
-{
- TMatrix3D src, tmp;
- float det;
- uint8_t i;
-
- /* transpose matrix */
- for (i = 0; i < 4; i++)
- {
- src.coef[i] = org->coef[i*4];
- src.coef[i + 4] = org->coef[i*4 + 1];
- src.coef[i + 8] = org->coef[i*4 + 2];
- src.coef[i + 12] = org->coef[i*4 + 3];
- }
-
- /* calculate pairs for first 8 elements (cofactors) */
- tmp.coef[0] = src.coef[10] * src.coef[15];
- tmp.coef[1] = src.coef[11] * src.coef[14];
- tmp.coef[2] = src.coef[9] * src.coef[15];
- tmp.coef[3] = src.coef[11] * src.coef[13];
- tmp.coef[4] = src.coef[9] * src.coef[14];
- tmp.coef[5] = src.coef[10] * src.coef[13];
- tmp.coef[6] = src.coef[8] * src.coef[15];
- tmp.coef[7] = src.coef[11] * src.coef[12];
- tmp.coef[8] = src.coef[8] * src.coef[14];
- tmp.coef[9] = src.coef[10] * src.coef[12];
- tmp.coef[10] = src.coef[8] * src.coef[13];
- tmp.coef[11] = src.coef[9] * src.coef[12];
- /* calculate first 8 elements (cofactors) */
- inv->coef[0] = (tmp.coef[0]*src.coef[5] + tmp.coef[3]*src.coef[6] + tmp.coef[4]*src.coef[7]) - (tmp.coef[1]*src.coef[5] + tmp.coef[2]*src.coef[6] + tmp.coef[5]*src.coef[7]);
- inv->coef[1] = (tmp.coef[1]*src.coef[4] + tmp.coef[6]*src.coef[6] + tmp.coef[9]*src.coef[7]) - (tmp.coef[0]*src.coef[4] + tmp.coef[7]*src.coef[6] + tmp.coef[8]*src.coef[7]);
- inv->coef[2] = (tmp.coef[2]*src.coef[4] + tmp.coef[7]*src.coef[5] + tmp.coef[10]*src.coef[7]) - (tmp.coef[3]*src.coef[4] + tmp.coef[6]*src.coef[5] + tmp.coef[11]*src.coef[7]);
- inv->coef[3] = (tmp.coef[5]*src.coef[4] + tmp.coef[8]*src.coef[5] + tmp.coef[11]*src.coef[6]) - (tmp.coef[4]*src.coef[4] + tmp.coef[9]*src.coef[5] + tmp.coef[10]*src.coef[6]);
- inv->coef[4] = (tmp.coef[1]*src.coef[1] + tmp.coef[2]*src.coef[2] + tmp.coef[5]*src.coef[3]) - (tmp.coef[0]*src.coef[1] + tmp.coef[3]*src.coef[2] + tmp.coef[4]*src.coef[3]);
- inv->coef[5] = (tmp.coef[0]*src.coef[0] + tmp.coef[7]*src.coef[2] + tmp.coef[8]*src.coef[3]) - (tmp.coef[1]*src.coef[0] + tmp.coef[6]*src.coef[2] + tmp.coef[9]*src.coef[3]);
- inv->coef[6] = (tmp.coef[3]*src.coef[0] + tmp.coef[6]*src.coef[1] + tmp.coef[11]*src.coef[3]) - (tmp.coef[2]*src.coef[0] + tmp.coef[7]*src.coef[1] + tmp.coef[10]*src.coef[3]);
- inv->coef[7] = (tmp.coef[4]*src.coef[0] + tmp.coef[9]*src.coef[1] + tmp.coef[10]*src.coef[2]) - (tmp.coef[5]*src.coef[0] + tmp.coef[8]*src.coef[1] + tmp.coef[11]*src.coef[2]);
- /* calculate pairs for second 8 elements (cofactors) */
- tmp.coef[0] = src.coef[2]*src.coef[7];
- tmp.coef[1] = src.coef[3]*src.coef[6];
- tmp.coef[2] = src.coef[1]*src.coef[7];
- tmp.coef[3] = src.coef[3]*src.coef[5];
- tmp.coef[4] = src.coef[1]*src.coef[6];
- tmp.coef[5] = src.coef[2]*src.coef[5];
- //Streaming SIMD Extensions - Inverse of 4x4 Matrix 8
- tmp.coef[6] = src.coef[0]*src.coef[7];
- tmp.coef[7] = src.coef[3]*src.coef[4];
- tmp.coef[8] = src.coef[0]*src.coef[6];
- tmp.coef[9] = src.coef[2]*src.coef[4];
- tmp.coef[10] = src.coef[0]*src.coef[5];
- tmp.coef[11] = src.coef[1]*src.coef[4];
- /* calculate second 8 elements (cofactors) */
- inv->coef[8] = (tmp.coef[0]*src.coef[13] + tmp.coef[3]*src.coef[14] + tmp.coef[4]*src.coef[15]) - (tmp.coef[1]*src.coef[13] + tmp.coef[2]*src.coef[14] + tmp.coef[5]*src.coef[15]);
- inv->coef[9] = (tmp.coef[1]*src.coef[12] + tmp.coef[6]*src.coef[14] + tmp.coef[9]*src.coef[15]) - (tmp.coef[0]*src.coef[12] + tmp.coef[7]*src.coef[14] + tmp.coef[8]*src.coef[15]);
- inv->coef[10] = (tmp.coef[2]*src.coef[12] + tmp.coef[7]*src.coef[13] + tmp.coef[10]*src.coef[15]) - (tmp.coef[3]*src.coef[12] + tmp.coef[6]*src.coef[13] + tmp.coef[11]*src.coef[15]);
- inv->coef[11] = (tmp.coef[5]*src.coef[12] + tmp.coef[8]*src.coef[13] + tmp.coef[11]*src.coef[14]) - (tmp.coef[4]*src.coef[12] + tmp.coef[9]*src.coef[13] + tmp.coef[10]*src.coef[14]);
- inv->coef[12] = (tmp.coef[2]*src.coef[10] + tmp.coef[5]*src.coef[11] + tmp.coef[1]*src.coef[9]) - (tmp.coef[4]*src.coef[11] + tmp.coef[0]*src.coef[9] + tmp.coef[3]*src.coef[10]);
- inv->coef[13] = (tmp.coef[8]*src.coef[11] + tmp.coef[0]*src.coef[8] + tmp.coef[7]*src.coef[10]) - (tmp.coef[6]*src.coef[10] + tmp.coef[9]*src.coef[11] + tmp.coef[1]*src.coef[8]);
- inv->coef[14] = (tmp.coef[6]*src.coef[9] + tmp.coef[11]*src.coef[11] + tmp.coef[3]*src.coef[8]) - (tmp.coef[10]*src.coef[11] + tmp.coef[2]*src.coef[8] + tmp.coef[7]*src.coef[9]);
- inv->coef[15] = (tmp.coef[10]*src.coef[10] + tmp.coef[4]*src.coef[8] + tmp.coef[9]*src.coef[9]) - (tmp.coef[8]*src.coef[9] + tmp.coef[11]*src.coef[10] + tmp.coef[5]*src.coef[8]);
- /* calculate determinant */
- det = src.coef[0]*inv->coef[0] + src.coef[1]*inv->coef[1] + src.coef[2]*inv->coef[2] + src.coef[3]*inv->coef[3];
- /* calculate matrix inverse */
- if (det == 0)
- {
- det = 0;
- return 0x00;
- }
- else
- {
- det = 1 / det;
- }
-
- for (i = 0; i < 16; i++)
- inv->coef[i]*=det;
-
- return 0x01;
-}
-
-void matrix3d_set_transform(TMatrix3D *matrix, TPoint3D *point, TVector3D *vector)
-{
- float Cx = cos(vector->x * 3.141592 / 180.0);
- float Cy = cos(vector->y * 3.141592 / 180.0);
- float Cz = cos(vector->z * 3.141592 / 180.0);
- float Sx = sin(vector->x * 3.141592 / 180.0);
- float Sy = sin(vector->y * 3.141592 / 180.0);
- float Sz = sin(vector->z * 3.141592 / 180.0);
-
- matrix3d_identity(matrix);
- matrix->coef[0] = Cz * Cy;
- matrix->coef[1] = Cz * Sy * Sx - Sz * Cx;
- matrix->coef[2] = Cz * Sy * Cx + Sz * Sx;
- matrix->coef[3] = point->x;
- matrix->coef[4] = Sz * Cy;
- matrix->coef[5] = Sz * Sy * Sx + Cz * Cx;
- matrix->coef[6] = Sz * Sy * Cx - Cz * Sx;
- matrix->coef[7] = point->y;
- matrix->coef[8] = -Sy;
- matrix->coef[9] = Cy * Sx;
- matrix->coef[10] = Cy * Cx;
- matrix->coef[11] = point->z;
-}
-
-void matrix3d_mult(TMatrix3D *dst,TMatrix3D *a,TMatrix3D *b)
-{
- uint8_t i,j,c;
-
- for(j = 0; j < 4; j++)
- {
- for(i = 0; i < 4; i++)
- {
- for(c = 0; c < 4; c++)
- {
- dst->coef[j*4+i] += a->coef[j*4+c] * b->coef[c*4+i];
- }
- }
- }
-}
+#include "darwin_math.h"
+
+#include <math.h>
+
+// point functions
+void point3d_init(TPoint3D *point)
+{
+ point->x=0.0;
+ point->y=0.0;
+ point->z=0.0;
+}
+
+void point3d_load(TPoint3D *point, float x, float y,float z)
+{
+ point->x=x;
+ point->y=y;
+ point->z=z;
+}
+
+void point3d_copy(TPoint3D *point_dst, TPoint3D *point_src)
+{
+ point_dst->x=point_src->x;
+ point_dst->y=point_src->y;
+ point_dst->z=point_src->z;
+}
+
+// vector functions
+void vector3d_init(TVector3D *vector)
+{
+ vector->x=0.0;
+ vector->y=0.0;
+ vector->z=0.0;
+}
+
+void vector3d_load(TVector3D *vector, float x, float y, float z)
+{
+ vector->x=x;
+ vector->y=y;
+ vector->z=z;
+}
+
+void vector3d_copy(TVector3D *vector_dst, TVector3D *vector_src)
+{
+ vector_dst->x=vector_src->x;
+ vector_dst->y=vector_src->y;
+ vector_dst->z=vector_src->z;
+}
+
+float vector3d_length(TVector3D *vector)
+{
+ return sqrt(vector->x*vector->x + vector->y*vector->y + vector->z*vector->z);
+}
+
+// matrix functions
+void matrix3d_init(TMatrix3D *matrix)
+{
+ matrix3d_identity(matrix);
+}
+
+void matrix3d_copy(TMatrix3D *matrix_dst, TMatrix3D *matrix_src)
+{
+ matrix_dst->coef[m00]=matrix_src->coef[m00];
+ matrix_dst->coef[m01]=matrix_src->coef[m01];
+ matrix_dst->coef[m02]=matrix_src->coef[m02];
+ matrix_dst->coef[m03]=matrix_src->coef[m03];
+ matrix_dst->coef[m10]=matrix_src->coef[m10];
+ matrix_dst->coef[m11]=matrix_src->coef[m11];
+ matrix_dst->coef[m12]=matrix_src->coef[m12];
+ matrix_dst->coef[m13]=matrix_src->coef[m13];
+ matrix_dst->coef[m20]=matrix_src->coef[m20];
+ matrix_dst->coef[m21]=matrix_src->coef[m21];
+ matrix_dst->coef[m22]=matrix_src->coef[m22];
+ matrix_dst->coef[m23]=matrix_src->coef[m23];
+ matrix_dst->coef[m30]=matrix_src->coef[m30];
+ matrix_dst->coef[m31]=matrix_src->coef[m31];
+ matrix_dst->coef[m32]=matrix_src->coef[m32];
+ matrix_dst->coef[m33]=matrix_src->coef[m33];
+}
+
+void matrix3d_zero(TMatrix3D *matrix)
+{
+ matrix->coef[m00]=0.0;
+ matrix->coef[m01]=0.0;
+ matrix->coef[m02]=0.0;
+ matrix->coef[m03]=0.0;
+ matrix->coef[m10]=0.0;
+ matrix->coef[m11]=0.0;
+ matrix->coef[m12]=0.0;
+ matrix->coef[m13]=0.0;
+ matrix->coef[m20]=0.0;
+ matrix->coef[m21]=0.0;
+ matrix->coef[m22]=0.0;
+ matrix->coef[m23]=0.0;
+ matrix->coef[m30]=0.0;
+ matrix->coef[m31]=0.0;
+ matrix->coef[m32]=0.0;
+ matrix->coef[m33]=0.0;
+}
+
+void matrix3d_identity(TMatrix3D *matrix)
+{
+ matrix->coef[m00]=1.0;
+ matrix->coef[m01]=0.0;
+ matrix->coef[m02]=0.0;
+ matrix->coef[m03]=0.0;
+ matrix->coef[m10]=0.0;
+ matrix->coef[m11]=1.0;
+ matrix->coef[m12]=0.0;
+ matrix->coef[m13]=0.0;
+ matrix->coef[m20]=0.0;
+ matrix->coef[m21]=0.0;
+ matrix->coef[m22]=1.0;
+ matrix->coef[m23]=0.0;
+ matrix->coef[m30]=0.0;
+ matrix->coef[m31]=0.0;
+ matrix->coef[m32]=0.0;
+ matrix->coef[m33]=1.0;
+}
+
+uint8_t matrix3d_inverse(TMatrix3D *org, TMatrix3D *inv)
+{
+ TMatrix3D src, tmp;
+ float det;
+ uint8_t i;
+
+ /* transpose matrix */
+ for (i = 0; i < 4; i++)
+ {
+ src.coef[i] = org->coef[i*4];
+ src.coef[i + 4] = org->coef[i*4 + 1];
+ src.coef[i + 8] = org->coef[i*4 + 2];
+ src.coef[i + 12] = org->coef[i*4 + 3];
+ }
+
+ /* calculate pairs for first 8 elements (cofactors) */
+ tmp.coef[0] = src.coef[10] * src.coef[15];
+ tmp.coef[1] = src.coef[11] * src.coef[14];
+ tmp.coef[2] = src.coef[9] * src.coef[15];
+ tmp.coef[3] = src.coef[11] * src.coef[13];
+ tmp.coef[4] = src.coef[9] * src.coef[14];
+ tmp.coef[5] = src.coef[10] * src.coef[13];
+ tmp.coef[6] = src.coef[8] * src.coef[15];
+ tmp.coef[7] = src.coef[11] * src.coef[12];
+ tmp.coef[8] = src.coef[8] * src.coef[14];
+ tmp.coef[9] = src.coef[10] * src.coef[12];
+ tmp.coef[10] = src.coef[8] * src.coef[13];
+ tmp.coef[11] = src.coef[9] * src.coef[12];
+ /* calculate first 8 elements (cofactors) */
+ inv->coef[0] = (tmp.coef[0]*src.coef[5] + tmp.coef[3]*src.coef[6] + tmp.coef[4]*src.coef[7]) - (tmp.coef[1]*src.coef[5] + tmp.coef[2]*src.coef[6] + tmp.coef[5]*src.coef[7]);
+ inv->coef[1] = (tmp.coef[1]*src.coef[4] + tmp.coef[6]*src.coef[6] + tmp.coef[9]*src.coef[7]) - (tmp.coef[0]*src.coef[4] + tmp.coef[7]*src.coef[6] + tmp.coef[8]*src.coef[7]);
+ inv->coef[2] = (tmp.coef[2]*src.coef[4] + tmp.coef[7]*src.coef[5] + tmp.coef[10]*src.coef[7]) - (tmp.coef[3]*src.coef[4] + tmp.coef[6]*src.coef[5] + tmp.coef[11]*src.coef[7]);
+ inv->coef[3] = (tmp.coef[5]*src.coef[4] + tmp.coef[8]*src.coef[5] + tmp.coef[11]*src.coef[6]) - (tmp.coef[4]*src.coef[4] + tmp.coef[9]*src.coef[5] + tmp.coef[10]*src.coef[6]);
+ inv->coef[4] = (tmp.coef[1]*src.coef[1] + tmp.coef[2]*src.coef[2] + tmp.coef[5]*src.coef[3]) - (tmp.coef[0]*src.coef[1] + tmp.coef[3]*src.coef[2] + tmp.coef[4]*src.coef[3]);
+ inv->coef[5] = (tmp.coef[0]*src.coef[0] + tmp.coef[7]*src.coef[2] + tmp.coef[8]*src.coef[3]) - (tmp.coef[1]*src.coef[0] + tmp.coef[6]*src.coef[2] + tmp.coef[9]*src.coef[3]);
+ inv->coef[6] = (tmp.coef[3]*src.coef[0] + tmp.coef[6]*src.coef[1] + tmp.coef[11]*src.coef[3]) - (tmp.coef[2]*src.coef[0] + tmp.coef[7]*src.coef[1] + tmp.coef[10]*src.coef[3]);
+ inv->coef[7] = (tmp.coef[4]*src.coef[0] + tmp.coef[9]*src.coef[1] + tmp.coef[10]*src.coef[2]) - (tmp.coef[5]*src.coef[0] + tmp.coef[8]*src.coef[1] + tmp.coef[11]*src.coef[2]);
+ /* calculate pairs for second 8 elements (cofactors) */
+ tmp.coef[0] = src.coef[2]*src.coef[7];
+ tmp.coef[1] = src.coef[3]*src.coef[6];
+ tmp.coef[2] = src.coef[1]*src.coef[7];
+ tmp.coef[3] = src.coef[3]*src.coef[5];
+ tmp.coef[4] = src.coef[1]*src.coef[6];
+ tmp.coef[5] = src.coef[2]*src.coef[5];
+ //Streaming SIMD Extensions - Inverse of 4x4 Matrix 8
+ tmp.coef[6] = src.coef[0]*src.coef[7];
+ tmp.coef[7] = src.coef[3]*src.coef[4];
+ tmp.coef[8] = src.coef[0]*src.coef[6];
+ tmp.coef[9] = src.coef[2]*src.coef[4];
+ tmp.coef[10] = src.coef[0]*src.coef[5];
+ tmp.coef[11] = src.coef[1]*src.coef[4];
+ /* calculate second 8 elements (cofactors) */
+ inv->coef[8] = (tmp.coef[0]*src.coef[13] + tmp.coef[3]*src.coef[14] + tmp.coef[4]*src.coef[15]) - (tmp.coef[1]*src.coef[13] + tmp.coef[2]*src.coef[14] + tmp.coef[5]*src.coef[15]);
+ inv->coef[9] = (tmp.coef[1]*src.coef[12] + tmp.coef[6]*src.coef[14] + tmp.coef[9]*src.coef[15]) - (tmp.coef[0]*src.coef[12] + tmp.coef[7]*src.coef[14] + tmp.coef[8]*src.coef[15]);
+ inv->coef[10] = (tmp.coef[2]*src.coef[12] + tmp.coef[7]*src.coef[13] + tmp.coef[10]*src.coef[15]) - (tmp.coef[3]*src.coef[12] + tmp.coef[6]*src.coef[13] + tmp.coef[11]*src.coef[15]);
+ inv->coef[11] = (tmp.coef[5]*src.coef[12] + tmp.coef[8]*src.coef[13] + tmp.coef[11]*src.coef[14]) - (tmp.coef[4]*src.coef[12] + tmp.coef[9]*src.coef[13] + tmp.coef[10]*src.coef[14]);
+ inv->coef[12] = (tmp.coef[2]*src.coef[10] + tmp.coef[5]*src.coef[11] + tmp.coef[1]*src.coef[9]) - (tmp.coef[4]*src.coef[11] + tmp.coef[0]*src.coef[9] + tmp.coef[3]*src.coef[10]);
+ inv->coef[13] = (tmp.coef[8]*src.coef[11] + tmp.coef[0]*src.coef[8] + tmp.coef[7]*src.coef[10]) - (tmp.coef[6]*src.coef[10] + tmp.coef[9]*src.coef[11] + tmp.coef[1]*src.coef[8]);
+ inv->coef[14] = (tmp.coef[6]*src.coef[9] + tmp.coef[11]*src.coef[11] + tmp.coef[3]*src.coef[8]) - (tmp.coef[10]*src.coef[11] + tmp.coef[2]*src.coef[8] + tmp.coef[7]*src.coef[9]);
+ inv->coef[15] = (tmp.coef[10]*src.coef[10] + tmp.coef[4]*src.coef[8] + tmp.coef[9]*src.coef[9]) - (tmp.coef[8]*src.coef[9] + tmp.coef[11]*src.coef[10] + tmp.coef[5]*src.coef[8]);
+ /* calculate determinant */
+ det = src.coef[0]*inv->coef[0] + src.coef[1]*inv->coef[1] + src.coef[2]*inv->coef[2] + src.coef[3]*inv->coef[3];
+ /* calculate matrix inverse */
+ if (det == 0)
+ {
+ det = 0;
+ return 0x00;
+ }
+ else
+ {
+ det = 1 / det;
+ }
+
+ for (i = 0; i < 16; i++)
+ inv->coef[i]*=det;
+
+ return 0x01;
+}
+
+void matrix3d_set_transform(TMatrix3D *matrix, TPoint3D *point, TVector3D *vector)
+{
+ float Cx = cos(vector->x * 3.141592 / 180.0);
+ float Cy = cos(vector->y * 3.141592 / 180.0);
+ float Cz = cos(vector->z * 3.141592 / 180.0);
+ float Sx = sin(vector->x * 3.141592 / 180.0);
+ float Sy = sin(vector->y * 3.141592 / 180.0);
+ float Sz = sin(vector->z * 3.141592 / 180.0);
+
+ matrix3d_identity(matrix);
+ matrix->coef[0] = Cz * Cy;
+ matrix->coef[1] = Cz * Sy * Sx - Sz * Cx;
+ matrix->coef[2] = Cz * Sy * Cx + Sz * Sx;
+ matrix->coef[3] = point->x;
+ matrix->coef[4] = Sz * Cy;
+ matrix->coef[5] = Sz * Sy * Sx + Cz * Cx;
+ matrix->coef[6] = Sz * Sy * Cx - Cz * Sx;
+ matrix->coef[7] = point->y;
+ matrix->coef[8] = -Sy;
+ matrix->coef[9] = Cy * Sx;
+ matrix->coef[10] = Cy * Cx;
+ matrix->coef[11] = point->z;
+}
+
+void matrix3d_mult(TMatrix3D *dst,TMatrix3D *a,TMatrix3D *b)
+{
+ uint8_t i,j,c;
+
+ matrix3d_zero(dst);
+ for(j = 0; j < 4; j++)
+ {
+ for(i = 0; i < 4; i++)
+ {
+ for(c = 0; c < 4; c++)
+ {
+ dst->coef[j*4+i] += a->coef[j*4+c] * b->coef[c*4+i];
+ }
+ }
+ }
+}
diff --git a/src/dynamixel_master.c b/src/dynamixel_master.c
deleted file mode 100755
index d5f5072d1b874a2a077491c650d44c9538d97cae..0000000000000000000000000000000000000000
--- a/src/dynamixel_master.c
+++ /dev/null
@@ -1,451 +0,0 @@
-#include "dynamixel_master.h"
-#include "ram.h"
-#include "time.h"
-
-#define PIN_USART1_TX GPIO_Pin_6
-#define PIN_USART1_RX GPIO_Pin_7
-#define PIN_ENABLE_TXD GPIO_Pin_4
-#define PIN_ENABLE_RXD GPIO_Pin_5
-#define PIN_ENABLE_DXLPWR GPIO_Pin_8
-#define PORT_USART1_TX GPIOB
-#define PORT_USART1_RX GPIOB
-#define PORT_ENABLE_TXD GPIOB
-#define PORT_ENABLE_RXD GPIOB
-#define PORT_ENABLE_DXLPWR GPIOB
-
-#define MAX_BUFFER_LEN 1024
-
-// private variables
-uint16_t dyn_master_timeout;// answer reception timeout
-// input buffer
-uint8_t dyn_master_rx_buffer[MAX_BUFFER_LEN];
-uint16_t dyn_master_rx_num_data;
-// output buffer
-uint8_t dyn_master_tx_buffer[MAX_BUFFER_LEN];
-uint16_t dyn_master_tx_num_data;
-uint16_t dyn_master_tx_ptr;
-// instruction packet ready flag
-volatile uint8_t dyn_master_packet_ready;
-// sending status packet flag
-volatile uint8_t dyn_master_sending_packet;
-
-// private functions
-void dyn_parse_status_packet(void)
-{
- if(dyn_master_rx_num_data>3)// the length byte has been received
- {
- if(dyn_master_rx_num_data==(dyn_get_length(dyn_master_rx_buffer)+4))
- dyn_master_packet_ready=0x01;
- }
-}
-
-void dyn_master_send(void)
-{
- // wait until any previous transmission ends
- while(dyn_master_sending_packet);
- // send the first byte
- dyn_master_tx_num_data=dyn_get_length(dyn_master_tx_buffer)+4;
- dyn_master_sending_packet=0x01;
- USART_ITConfig(USART1, USART_IT_TC, ENABLE);
-}
-
-uint8_t dyn_master_receive(void)
-{
- int16_t timeout_left=dyn_master_timeout;
-
- // wait for the status packet
- while(!dyn_master_packet_ready)
- {
- delay_ms(10);
- timeout_left-=10;
- if(timeout_left<=0)
- return DYN_TIMEOUT;
- }
- dyn_master_packet_ready=0x00;
- dyn_master_rx_num_data=0x00;
- // check the input packet checksum
- if(dyn_check_checksum(dyn_master_rx_buffer)==0xFF)
- return dyn_get_status_error(dyn_master_rx_buffer);
- else
- return DYN_CHECKSUM_ERROR;
-}
-
-void dyn_master_enable_tx(void)
-{
- GPIO_ResetBits(PORT_ENABLE_RXD, PIN_ENABLE_RXD);
- GPIO_SetBits(PORT_ENABLE_TXD, PIN_ENABLE_TXD);
-}
-
-void dyn_master_enable_rx(void)
-{
- GPIO_ResetBits(PORT_ENABLE_TXD, PIN_ENABLE_TXD);
- GPIO_SetBits(PORT_ENABLE_RXD, PIN_ENABLE_RXD);
-}
-
-uint8_t dyn_master_read(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
-{
- uint8_t error;
-
- // generate the read packet for the desired device
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_READ);
- // set the device id
- if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
- {
- // set the length to read
- dyn_set_read_length(dyn_master_tx_buffer,length);
- // sert the start address to start reading
- dyn_set_read_address(dyn_master_tx_buffer,address);
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- if(error==DYN_NO_ERROR)
- {
- // read the data from the status packet
- dyn_get_status_data(dyn_master_rx_buffer,data);
- }
- return error;
- }
- else
- return error;
-}
-
-uint8_t dyn_master_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
-{
- uint8_t error;
-
- // generate the read packet for the desired device
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_WRITE);
- // set the device id
- if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
- {
- // sert the start address to start reading
- dyn_set_read_address(dyn_master_tx_buffer,address);
- // set the data to write and its length
- dyn_set_write_data(dyn_master_tx_buffer,length,data);
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- return error;
- }
- else
- return error;
-}
-
-// interrupt handlers
-/**
- * @brief This function handles USART1 global interrupt request.
- * @param None
- * @retval None
- */
-void USART1_IRQHandler(void)
-{
- if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
- {
- /* Read one byte from the receive data register */
- dyn_master_rx_buffer[dyn_master_rx_num_data++] = USART_ReceiveData(USART1);
- dyn_parse_status_packet();
- }
-
- if(USART_GetITStatus(USART1, USART_IT_TC) != RESET)
- {
- if(dyn_master_tx_num_data==0x00)// there is no more data to be sent
- {
- dyn_master_tx_ptr=0x00;
- dyn_master_sending_packet=0x00;
- // disable interrupts
- USART_ITConfig(USART1, USART_IT_TC, DISABLE);
- }
- else// there is still data to be sent
- {
- dyn_master_tx_num_data--;
- USART_SendData(USART1,dyn_master_tx_buffer[dyn_master_tx_ptr++]);// send the next_byte
- }
- }
-}
-
-// dynamixel master functions
-void dyn_master_init(void)
-{
- uint16_t i;
- GPIO_InitTypeDef GPIO_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
- USART_InitTypeDef USART_InitStructure;
-
- // configure the GPIO pins
- /* Enable the USART1 Pins Software Remapping */
- GPIO_PinRemapConfig(GPIO_Remap_USART1, ENABLE);
-
- /* Configure USART1 Rx as input floating */
- GPIO_InitStructure.GPIO_Pin = PIN_USART1_RX;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
- GPIO_Init(PORT_USART1_RX, &GPIO_InitStructure);
-
- /* Configure USART1 Tx as alternate function push-pull */
- GPIO_InitStructure.GPIO_Pin = PIN_USART1_TX;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
- GPIO_Init(PORT_USART1_TX, &GPIO_InitStructure);
-
- /* configure the control pins */
- GPIO_InitStructure.GPIO_Pin = PIN_ENABLE_TXD | PIN_ENABLE_RXD | PIN_ENABLE_DXLPWR ;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_Init(GPIOB, &GPIO_InitStructure);
-
- dyn_master_enable_rx();
-
- dyn_master_timeout=0x00;
- ram_write_byte(CM730_DXL_POWER,0x00);
- // initialize the buffers
- for(i=0;i<MAX_BUFFER_LEN;i++)
- {
- dyn_master_rx_buffer[i]=0x00;
- dyn_master_tx_buffer[i]=0x00;
- }
- dyn_master_rx_num_data=0x00;
- dyn_master_tx_num_data=0x00;
- dyn_master_tx_ptr=0x00;
- // initialize the flags
- dyn_master_packet_ready=0x00;
- dyn_master_sending_packet=0x00;
-
- USART_DeInit(USART1);
- USART_StructInit(&USART_InitStructure);
- // configure the serial port
- USART_InitStructure.USART_BaudRate = 1000000;
- USART_InitStructure.USART_WordLength = USART_WordLength_8b;
- USART_InitStructure.USART_StopBits = USART_StopBits_1;
- USART_InitStructure.USART_Parity = USART_Parity_No;
- USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
- USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
- USART_Init(USART1, &USART_InitStructure);
-
- // configure the interrupts
- NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
- USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
- USART_ITConfig(USART1, USART_IT_ORE, DISABLE);
- USART_ITConfig(USART1, USART_IT_TXE, DISABLE);
- USART_ITConfig(USART1, USART_IT_TC, ENABLE);
- USART_ITConfig(USART1, USART_IT_CTS, DISABLE);
- USART_ITConfig(USART1, USART_IT_LBD, DISABLE);
- USART_ITConfig(USART1, USART_IT_IDLE, DISABLE);
- USART_ITConfig(USART1, USART_IT_PE, DISABLE);
- USART_ITConfig(USART1, USART_IT_ERR, DISABLE);
-
- /* Enable the USART1 */
- USART_Cmd(USART1, ENABLE);
-}
-
-void dyn_master_flush(void)
-{
- // flush only the reception buffer to avoid interrupting a sync_write command
- dyn_master_rx_num_data=0x00;
-// dyn_master_tx_num_data=0x00;
-// dyn_master_tx_ptr=0x00;
- // initialize the flags
- dyn_master_packet_ready=0x00;
-// dyn_master_sending_packet=0x00;
-}
-
-void dyn_master_power_on(void)
-{
- GPIO_SetBits(PORT_ENABLE_DXLPWR, PIN_ENABLE_DXLPWR);
- ram_write_byte(CM730_DXL_POWER,0x01);
-}
-
-void dyn_master_power_off(void)
-{
- GPIO_ResetBits(PORT_ENABLE_DXLPWR, PIN_ENABLE_DXLPWR);
- ram_write_byte(CM730_DXL_POWER,0x00);
-}
-
-uint8_t dyn_master_is_powered_on(void)
-{
- uint8_t power;
-
- ram_read_byte(CM730_DXL_POWER,&power);
- return power;
-}
-
-void dyn_master_set_timeout(uint16_t timeout_ms)
-{
- // save the desired timeout value
- dyn_master_timeout=timeout_ms;
-}
-
-void dyn_master_scan(uint8_t *num,uint8_t *ids)
-{
- uint8_t i;
-
- *num=0;
- for(i=0;i<30;i++)
- {
- if(dyn_master_ping(i)==DYN_NO_ERROR)// the device exists
- {
- ids[*num]=i;
- (*num)++;
- }
- }
-}
-
-uint8_t dyn_master_ping(uint8_t id)
-{
- uint8_t error;
-
- // generate the ping packet for the desired device
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_PING);
- // set the device id
- if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
- {
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- return error;
- }
- else
- return error;
-}
-
-uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data)
-{
- return dyn_master_read(id,address,1,data);
-}
-
-uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data)
-{
- uint8_t value[2];
- uint8_t error;
-
- error=dyn_master_read(id,address,2,value);
- if(error==DYN_NO_ERROR)
- (*data)=value[0]+value[1]*256;
-
- return error;
-}
-
-uint8_t dyn_master_read_table(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
-{
- return dyn_master_read(id,address,length,data);
-}
-
-uint8_t dyn_master_write_byte(uint8_t id, uint8_t address, uint8_t data)
-{
- return dyn_master_write(id,address,1,&data);
-}
-
-uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data)
-{
- uint8_t value[2];
-
- value[0]=data%256;
- value[1]=data/256;
- return dyn_master_write(id,address,2,value);
-}
-
-uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
-{
- return dyn_master_write(id,address,length,data);
-}
-
-uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
-{
- return DYN_SUCCESS;
-}
-
-void dyn_master_action(void)
-{
-
-}
-
-void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, uint8_t *data)
-{
- uint8_t i;
-
- // generate the sync write packet
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_SYNC_WRITE);
- // set the start address
- dyn_set_write_address(dyn_master_tx_buffer,address);
- // set the data length
- dyn_set_sync_write_length(dyn_master_tx_buffer,length);
- // load the data for each device
- for(i=0;i<num;i++)
- dyn_set_sync_write_data(dyn_master_tx_buffer,ids[i],&data[i*length]);
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
-// while(dyn_master_sending_packet);
-}
-
-uint8_t dyn_master_reset(uint8_t id)
-{
- return DYN_SUCCESS;
-}
-
-// repeater functions
-uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet)
-{
- uint8_t error;
-
- // generate the read packet for the desired device
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // copy the contents of the instruction packet to the output buffer
- dyn_copy_packet(inst_packet,dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
- while(dyn_master_sending_packet);
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- if(error!=DYN_TIMEOUT)
- {
- // copy the contents of the status of the packet to the output buffer
- dyn_copy_packet(dyn_master_rx_buffer,status_packet);
- }
-
- return error;
-}
-
diff --git a/src/dynamixel_master_uart_dma.c b/src/dynamixel_master_uart_dma.c
new file mode 100755
index 0000000000000000000000000000000000000000..d384c7fe76ed5a0d14b2a8c8230e645110840f3d
--- /dev/null
+++ b/src/dynamixel_master_uart_dma.c
@@ -0,0 +1,571 @@
+#include "dynamixel_master_uart_dma.h"
+#include "time.h"
+#include "gpio.h"
+
+#define USART USART1
+#define USART_CLK RCC_APB2Periph_USART1
+#define USART_CLK_INIT RCC_APB2PeriphClockCmd
+#define USART_IRQn USART1_IRQn
+#define USART_IRQHandler USART1_IRQHandler
+
+#define USART_TX_PIN GPIO_Pin_6
+#define USART_TX_GPIO_PORT GPIOB
+#define USART_TX_GPIO_CLK RCC_APB2Periph_GPIOB
+#define USART_TX_SOURCE GPIO_PinSource6
+
+#define USART_RX_PIN GPIO_Pin_7
+#define USART_RX_GPIO_PORT GPIOB
+#define USART_RX_GPIO_CLK RCC_APB2Periph_GPIOB
+#define USART_RX_SOURCE GPIO_PinSource7
+
+#define USART_TX_EN_PIN GPIO_Pin_4
+#define USART_TX_EN_GPIO_PORT GPIOB
+#define USART_TX_EN_GPIO_CLK RCC_APB2Periph_GPIOB
+#define USART_TX_EN_SOURCE GPIO_PinSource4
+
+#define USART_RX_EN_PIN GPIO_Pin_5
+#define USART_RX_EN_GPIO_PORT GPIOB
+#define USART_RX_EN_GPIO_CLK RCC_APB2Periph_GPIOB
+#define USART_RX_EN_SOURCE GPIO_PinSource5
+
+/* DMA configuration */
+#define USART_DR_ADDRESS ((uint32_t)USART1 + 0x04)
+#define USART_DMA DMA1
+#define USART_DMA_CLK RCC_AHBPeriph_DMA1
+
+#define USART_TX_DMA_CHANNEL DMA1_Channel4
+#define USART_TX_DMA_FLAG_GLIF DMA1_FLAG_GL4
+#define USART_TX_DMA_FLAG_TEIF DMA1_FLAG_TE4
+#define USART_TX_DMA_FLAG_HTIF DMA1_FLAG_HT4
+#define USART_TX_DMA_FLAG_TCIF DMA1_FLAG_TC4
+
+#define USART_RX_DMA_CHANNEL DMA1_Channel5
+#define USART_RX_DMA_FLAG_GLIF DMA1_FLAG_GL5
+#define USART_RX_DMA_FLAG_TEIF DMA1_FLAG_TE5
+#define USART_RX_DMA_FLAG_HTIF DMA1_FLAG_HT5
+#define USART_RX_DMA_FLAG_TCIF DMA1_FLAG_TC5
+
+#define USART_DMA_TX_IRQn DMA1_Channel4_IRQn
+#define USART_DMA_RX_IRQn DMA1_Channel5_IRQn
+#define USART_DMA_TX_IRQHandler DMA1_Channel4_IRQHandler
+#define USART_DMA_RX_IRQHandler DMA1_Channel5_IRQHandler
+
+#define POWER_GPIO_CLK RCC_APB2Periph_GPIOB
+#define POWER_PIN GPIO_Pin_8
+#define POWER_GPIO_PORT GPIOB
+#define POWER_SOURCE GPIO_PinSource8
+
+
+#define MAX_BUFFER_LEN 1024
+
+// private variables
+uint16_t dyn_master_timeout;// answer reception timeout
+// input buffer
+uint8_t dyn_master_rx_buffer[MAX_BUFFER_LEN];
+volatile uint8_t dyn_master_receiving_header;
+// output buffer
+uint8_t dyn_master_tx_buffer[MAX_BUFFER_LEN];
+// instruction packet ready flag
+volatile uint8_t dyn_master_packet_ready;
+// sending status packet flag
+volatile uint8_t dyn_master_sending_packet;
+// no answer for sync write operation
+uint8_t dyn_master_no_answer;
+// power enabled variable
+uint8_t dyn_master_power_enabled;
+// DMA initialization data structures
+DMA_InitTypeDef DMA_TX_InitStructure;
+DMA_InitTypeDef DMA_RX_InitStructure;
+
+// private functions
+void dyn_master_reset(void)
+{
+ DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_TC,DISABLE);
+ USART_DMACmd(USART, USART_DMAReq_Rx, DISABLE);
+ DMA_Cmd(USART_RX_DMA_CHANNEL,DISABLE);
+ /* wait until the transaction ends */
+ DMA_ClearFlag(USART_RX_DMA_FLAG_GLIF);
+ DMA_ClearITPendingBit(USART_RX_DMA_FLAG_GLIF);
+ dyn_master_receiving_header=0x01;
+ dyn_master_packet_ready=0x00;
+}
+
+void dyn_master_send(void)
+{
+ // wait until any previous transmission ends
+ while(dyn_master_sending_packet);
+ // set the DMA transfer
+ DMA_SetCurrDataCounter(USART_TX_DMA_CHANNEL,dyn_get_length(dyn_master_tx_buffer)+4);
+ DMA_Cmd(USART_TX_DMA_CHANNEL,ENABLE);
+ USART_ClearFlag(USART,USART_FLAG_TC);
+ USART_DMACmd(USART, USART_DMAReq_Tx, ENABLE);
+ dyn_master_sending_packet=0x01;
+}
+
+uint8_t dyn_master_receive(void)
+{
+ int16_t timeout_left=dyn_master_timeout;
+
+ // wait for the status packet
+ while(!dyn_master_packet_ready)
+ {
+ delay_ms(1);
+ timeout_left-=1;
+ if(timeout_left<=0)
+ {
+ dyn_master_reset();
+ return DYN_TIMEOUT;
+ }
+ }
+ dyn_master_packet_ready=0x00;
+ // check the input packet checksum
+ if(dyn_check_checksum(dyn_master_rx_buffer)==0xFF)
+ return dyn_get_status_error(dyn_master_rx_buffer);
+ else
+ return DYN_CHECKSUM_ERROR;
+}
+
+void dyn_master_enable_tx(void)
+{
+ GPIO_ResetBits(USART_RX_EN_GPIO_PORT, USART_RX_EN_PIN);
+ GPIO_SetBits(USART_TX_EN_GPIO_PORT, USART_TX_EN_PIN);
+}
+
+void dyn_master_enable_rx(void)
+{
+ GPIO_ResetBits(USART_TX_EN_GPIO_PORT, USART_TX_EN_PIN);
+ GPIO_SetBits(USART_RX_EN_GPIO_PORT, USART_RX_EN_PIN);
+}
+
+uint8_t dyn_master_read(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
+{
+ uint8_t error;
+
+ // wait for the transmission to end
+ while(dyn_master_sending_packet);
+ // generate the read packet for the desired device
+ dyn_init_instruction_packet(dyn_master_tx_buffer);
+ // set the ping instruction
+ dyn_set_instruction(dyn_master_tx_buffer,DYN_READ);
+ // set the device id
+ if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
+ {
+ // set the length to read
+ dyn_set_read_length(dyn_master_tx_buffer,length);
+ // sert the start address to start reading
+ dyn_set_read_address(dyn_master_tx_buffer,address);
+ // set the checksum
+ dyn_set_checksum(dyn_master_tx_buffer);
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ dyn_master_send();
+ // wait for the transmission to end
+ while(dyn_master_sending_packet);
+ dyn_master_enable_rx();
+ // wait for the replay within the given timeout
+ error=dyn_master_receive();
+ if(error==DYN_NO_ERROR)
+ dyn_get_status_data(dyn_master_rx_buffer,data);// read the data from the status packet
+ else
+ dyn_master_reset();
+ return error;
+ }
+ else
+ return error;
+}
+
+uint8_t dyn_master_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
+{
+ uint8_t error;
+
+ // wait for the transmission to end
+ while(dyn_master_sending_packet);
+ // generate the read packet for the desired device
+ dyn_init_instruction_packet(dyn_master_tx_buffer);
+ // set the ping instruction
+ dyn_set_instruction(dyn_master_tx_buffer,DYN_WRITE);
+ // set the device id
+ if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
+ {
+ // sert the start address to start reading
+ dyn_set_read_address(dyn_master_tx_buffer,address);
+ // set the data to write and its length
+ dyn_set_write_data(dyn_master_tx_buffer,length,data);
+ // set the checksum
+ dyn_set_checksum(dyn_master_tx_buffer);
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ dyn_master_send();
+ // wait for the transmission to end
+ while(dyn_master_sending_packet);
+ dyn_master_enable_rx();
+ // wait for the replay within the given timeout
+ error=dyn_master_receive();
+ if(error!=DYN_NO_ERROR)
+ dyn_master_reset();
+ return error;
+ }
+ else
+ return error;
+}
+
+// interrupt handlers
+void USART_IRQHandler(void)
+{
+ if(USART_GetITStatus(USART, USART_IT_TC) != RESET)
+ {
+ USART_ClearFlag(USART,USART_FLAG_TC);
+ USART_ITConfig(USART, USART_IT_TC, DISABLE);
+ dyn_master_sending_packet=0x00;
+ if(dyn_master_no_answer)
+ dyn_master_no_answer=0x00;
+ else
+ {
+ // set up the DMA RX transaction
+ DMA_RX_InitStructure.DMA_BufferSize = 4;
+ DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_master_rx_buffer;
+ DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure);
+ DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_TC,ENABLE);
+ DMA_Cmd(USART_RX_DMA_CHANNEL,ENABLE);
+ USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
+ dyn_master_receiving_header=0x01;
+ }
+ }
+}
+
+void USART_DMA_TX_IRQHandler(void)
+{
+ DMA_Cmd(USART_TX_DMA_CHANNEL,DISABLE);
+ DMA_ClearFlag(USART_TX_DMA_FLAG_GLIF);
+ DMA_ClearITPendingBit(USART_TX_DMA_FLAG_GLIF);
+ USART_ITConfig(USART, USART_IT_TC, ENABLE);
+}
+
+void USART_DMA_RX_IRQHandler(void)
+{
+ if(dyn_master_receiving_header==0x01)
+ {
+ DMA_Cmd(USART_RX_DMA_CHANNEL,DISABLE);
+ DMA_RX_InitStructure.DMA_BufferSize = dyn_get_length(dyn_master_rx_buffer);
+ DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&dyn_master_rx_buffer[4];
+ DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure);
+ DMA_Cmd(USART_RX_DMA_CHANNEL,ENABLE);
+ USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
+ DMA_ClearFlag(USART_RX_DMA_FLAG_GLIF);
+ DMA_ClearITPendingBit(USART_RX_DMA_FLAG_GLIF);
+ dyn_master_receiving_header=0x00;
+ }
+ else
+ {
+ DMA_Cmd(USART_RX_DMA_CHANNEL,DISABLE);
+ DMA_ClearFlag(USART_RX_DMA_FLAG_GLIF);
+ DMA_ClearITPendingBit(USART_RX_DMA_FLAG_GLIF);
+ DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_TC,DISABLE);
+ dyn_master_packet_ready=0x01;
+ }
+}
+
+// dynamixel master functions
+void dyn_master_init(void)
+{
+ uint16_t i;
+ GPIO_InitTypeDef GPIO_InitStructure;
+ NVIC_InitTypeDef NVIC_InitStructure;
+ USART_InitTypeDef USART_InitStructure;
+
+ /* Enable GPIO clock */
+ USART_CLK_INIT(USART_CLK, ENABLE);
+ RCC_APB1PeriphClockCmd(USART_TX_GPIO_CLK | USART_RX_GPIO_CLK | USART_TX_EN_GPIO_CLK | USART_RX_EN_GPIO_CLK, ENABLE);
+ RCC_APB1PeriphClockCmd(POWER_GPIO_CLK, ENABLE);
+ // configure the GPIO pins
+
+ /* Connect USART pins to AF7 */
+ GPIO_PinRemapConfig(GPIO_Remap_USART1, ENABLE);
+
+ /* Configure USART Tx and Rx as alternate function push-pull */
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_InitStructure.GPIO_Pin = USART_TX_PIN;
+ GPIO_Init(USART_TX_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = USART_RX_PIN;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+ GPIO_Init(USART_RX_GPIO_PORT, &GPIO_InitStructure);
+
+ /* configure the control pins */
+ GPIO_InitStructure.GPIO_Pin = USART_TX_EN_PIN;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+ GPIO_Init(USART_TX_EN_GPIO_PORT, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = USART_RX_EN_PIN;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+ GPIO_Init(USART_RX_EN_GPIO_PORT, &GPIO_InitStructure);
+
+ dyn_master_enable_rx();
+
+ dyn_master_timeout=0x00;
+ // initialize the buffers
+ for(i=0;i<MAX_BUFFER_LEN;i++)
+ {
+ dyn_master_rx_buffer[i]=0x00;
+ dyn_master_tx_buffer[i]=0x00;
+ }
+ // initialize the flags
+ dyn_master_packet_ready=0x00;
+ dyn_master_sending_packet=0x00;
+ dyn_master_receiving_header=0x01;
+ dyn_master_no_answer=0x00;
+
+ USART_DeInit(USART);
+ USART_StructInit(&USART_InitStructure);
+ // configure the serial port
+ USART_InitStructure.USART_BaudRate = 1000000;
+ USART_InitStructure.USART_WordLength = USART_WordLength_8b;
+ USART_InitStructure.USART_StopBits = USART_StopBits_1;
+ USART_InitStructure.USART_Parity = USART_Parity_No;
+ USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
+ USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
+ USART_Init(USART, &USART_InitStructure);
+ NVIC_InitStructure.NVIC_IRQChannel = USART_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+ USART_ITConfig(USART, USART_IT_RXNE | USART_IT_ORE | USART_IT_TXE | USART_IT_CTS | USART_IT_LBD | USART_IT_IDLE | USART_IT_PE | USART_IT_ERR | USART_IT_TC, DISABLE);
+
+ // configure the DMA channels
+ /* Configure TX DMA */
+ RCC_AHBPeriphClockCmd(USART_DMA_CLK, ENABLE);
+ DMA_DeInit(USART_TX_DMA_CHANNEL);
+ DMA_TX_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+ DMA_TX_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+ DMA_TX_InitStructure.DMA_Mode = DMA_Mode_Normal;
+ DMA_TX_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) (&(USART->DR));
+ DMA_TX_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+ DMA_TX_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+ DMA_TX_InitStructure.DMA_Priority = DMA_Priority_High;
+ DMA_TX_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
+ DMA_TX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_master_tx_buffer;
+ DMA_Init(USART_TX_DMA_CHANNEL,&DMA_TX_InitStructure);
+ /* initialize DMA interrupts */
+ NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_TX_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+ DMA_ITConfig(USART_TX_DMA_CHANNEL,DMA_IT_TC,ENABLE);
+ DMA_ITConfig(USART_TX_DMA_CHANNEL,DMA_IT_HT | DMA_IT_TE,DISABLE);
+
+ /* Configure RX DMA */
+ DMA_DeInit(USART_RX_DMA_CHANNEL);
+ DMA_RX_InitStructure.DMA_BufferSize = 4;// transfer the first 3 bytes
+ DMA_RX_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+ DMA_RX_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+ DMA_RX_InitStructure.DMA_Mode = DMA_Mode_Normal;
+ DMA_RX_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) (&(USART->DR));
+ DMA_RX_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+ DMA_RX_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+ DMA_RX_InitStructure.DMA_Priority = DMA_Priority_High;
+ DMA_RX_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
+ DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_master_rx_buffer;
+ DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure);
+ /* initialize DMA interrupts */
+ NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_RX_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+ DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_TC | DMA_IT_HT | DMA_IT_TE,DISABLE);
+
+ /* Enable the USART2 */
+ USART_Cmd(USART, ENABLE);
+
+ // initialize the power enable gpio
+ GPIO_InitStructure.GPIO_Pin = POWER_PIN;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(POWER_GPIO_PORT, &GPIO_InitStructure);
+
+ dyn_master_disable_power();
+}
+
+void dyn_master_flush(void)
+{
+ // flush only the reception buffer to avoid interrupting a sync_write command
+}
+
+void dyn_master_set_timeout(uint16_t timeout_ms)
+{
+ // save the desired timeout value
+ dyn_master_timeout=timeout_ms;
+}
+
+void dyn_master_scan(uint8_t *num,uint8_t *ids)
+{
+ uint8_t i;
+
+ *num=0;
+ for(i=0;i<254;i++)
+ {
+ if(dyn_master_ping(i)==DYN_NO_ERROR)// the device exists
+ {
+ ids[*num]=i;
+ (*num)++;
+ }
+ }
+}
+
+inline void dyn_master_enable_power(void)
+{
+ GPIO_SetBits(POWER_GPIO_PORT,POWER_PIN);
+ dyn_master_power_enabled=0x01;
+}
+
+inline void dyn_master_disable_power(void)
+{
+ GPIO_ResetBits(POWER_GPIO_PORT,POWER_PIN);
+ dyn_master_power_enabled=0x00;
+}
+
+uint8_t dyn_master_is_power_enabled(void)
+{
+ return dyn_master_power_enabled;
+}
+
+uint8_t dyn_master_ping(uint8_t id)
+{
+ uint8_t error;
+
+ // generate the ping packet for the desired device
+ dyn_init_instruction_packet(dyn_master_tx_buffer);
+ // set the ping instruction
+ dyn_set_instruction(dyn_master_tx_buffer,DYN_PING);
+ // set the device id
+ if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
+ {
+ // set the checksum
+ dyn_set_checksum(dyn_master_tx_buffer);
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ dyn_master_send();
+ // wait for the transmission to end
+ while (dyn_master_sending_packet==0x01);
+ dyn_master_enable_rx();
+ // wait for the replay within the given timeout
+ error=dyn_master_receive();
+ return error;
+ }
+ else
+ return error;
+}
+
+uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data)
+{
+ return dyn_master_read(id,address,1,data);
+}
+
+uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data)
+{
+ uint8_t value[2];
+ uint8_t error;
+
+ error=dyn_master_read(id,address,2,value);
+ if(error==DYN_NO_ERROR)
+ (*data)=value[0]+value[1]*256;
+
+ return error;
+}
+
+uint8_t dyn_master_read_table(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
+{
+ return dyn_master_read(id,address,length,data);
+}
+
+uint8_t dyn_master_write_byte(uint8_t id, uint8_t address, uint8_t data)
+{
+ return dyn_master_write(id,address,1,&data);
+}
+
+uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data)
+{
+ uint8_t value[2];
+
+ value[0]=data%256;
+ value[1]=data/256;
+ return dyn_master_write(id,address,2,value);
+}
+
+uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
+{
+ return dyn_master_write(id,address,length,data);
+}
+
+uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
+{
+ return DYN_SUCCESS;
+}
+
+void dyn_master_action(void)
+{
+
+}
+
+void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, uint8_t *data)
+{
+ uint8_t i;
+
+ // wait for the transmission to end
+ while(dyn_master_sending_packet);
+ dyn_master_no_answer=0x01;
+ // generate the sync write packet
+ dyn_init_instruction_packet(dyn_master_tx_buffer);
+ // set the ping instruction
+ dyn_set_instruction(dyn_master_tx_buffer,DYN_SYNC_WRITE);
+ // set the start address
+ dyn_set_write_address(dyn_master_tx_buffer,address);
+ // set the data length
+ dyn_set_sync_write_length(dyn_master_tx_buffer,length);
+ // load the data for each device
+ for(i=0;i<num;i++)
+ dyn_set_sync_write_data(dyn_master_tx_buffer,ids[i],&data[i*length]);
+ // set the checksum
+ dyn_set_checksum(dyn_master_tx_buffer);
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ dyn_master_send();
+ // wait for the transmission to end
+}
+
+// repeater functions
+uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet)
+{
+ uint8_t error;
+
+ // generate the read packet for the desired device
+ dyn_init_instruction_packet(dyn_master_tx_buffer);
+ // copy the contents of the instruction packet to the output buffer
+ dyn_copy_packet(inst_packet,dyn_master_tx_buffer);
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ dyn_master_send();
+ // wait for the transmission to end
+ while(dyn_master_sending_packet);
+ dyn_master_enable_rx();
+ // wait for the replay within the given timeout
+ error=dyn_master_receive();
+ if(error!=DYN_TIMEOUT)
+ {
+ // copy the contents of the status of the packet to the output buffer
+ dyn_copy_packet(dyn_master_rx_buffer,status_packet);
+ }
+
+ return error;
+}
+
diff --git a/src/dynamixel_slave.c b/src/dynamixel_slave.c
deleted file mode 100755
index d204d28ff322f93c8f4e928654e4ecb4e63a3431..0000000000000000000000000000000000000000
--- a/src/dynamixel_slave.c
+++ /dev/null
@@ -1,196 +0,0 @@
-#include "dynamixel_slave.h"
-
-#define PIN_USART3_TX GPIO_Pin_10
-#define PIN_USART3_RX GPIO_Pin_11
-#define PIN_nSLEEP GPIO_Pin_13
-#define PORT_USART3_TX GPIOB
-#define PORT_USART3_RX GPIOB
-#define PORT_nSLEEP GPIOA
-
-#define MAX_BUFFER_LEN 1024
-
-// private variables
-uint8_t dyn_slave_address;// this module slave address
-// input buffer
-uint8_t dyn_slave_rx_buffer[MAX_BUFFER_LEN];
-uint16_t dyn_slave_rx_num_data;
-// output buffer
-uint8_t dyn_slave_tx_buffer[MAX_BUFFER_LEN];
-uint16_t dyn_slave_tx_num_data;
-uint16_t dyn_slave_tx_ptr;
-// instruction packet ready flag
-volatile uint8_t dyn_slave_packet_ready;
-// sending status packet flag
-volatile uint8_t dyn_slave_sending_packet;
-
-// private functions
-void dyn_parse_inst_packet(void)
-{
- if(dyn_slave_rx_num_data>3)// the length byte has been received
- {
- if(dyn_slave_rx_num_data==(dyn_get_length(dyn_slave_rx_buffer)+4))// payload size plus header size
- dyn_slave_packet_ready=0x01;
- }
-}
-
-// interrupt handlers
-/**
- * @brief This function handles USART3 global interrupt request.
- * @param None
- * @retval None
- */
-void USART3_IRQHandler(void)
-{
- if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)// a byte has been received
- {
- /* Read one byte from the receive data register */
- dyn_slave_rx_buffer[dyn_slave_rx_num_data++] = USART_ReceiveData(USART3);
- dyn_parse_inst_packet();
- }
-
- if(USART_GetITStatus(USART3, USART_IT_TXE) != RESET)// a byte has been send
- {
- if(dyn_slave_tx_num_data==0x00)// there is no more data to be sent
- {
- dyn_slave_tx_ptr=0x00;
- dyn_slave_sending_packet=0x00;
- // disable interrupts
- USART_ITConfig(USART3, USART_IT_TXE, DISABLE);
- }
- else
- {
- USART_SendData(USART3,dyn_slave_tx_buffer[dyn_slave_tx_ptr++]);// send the next_byte
- dyn_slave_tx_num_data--;
- }
- }
-}
-
-// public functions
-void dyn_slave_init(void)
-{
- uint16_t i;
- GPIO_InitTypeDef GPIO_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
- USART_InitTypeDef USART_InitStructure;
-
- // initialize the buffers
- for(i=0;i<MAX_BUFFER_LEN;i++)
- {
- dyn_slave_rx_buffer[i]=0x00;
- dyn_slave_tx_buffer[i]=0x00;
- }
- dyn_slave_rx_num_data=0x00;
- dyn_slave_tx_num_data=0x00;
- dyn_slave_tx_ptr=0x00;
- // initialize the flags
- dyn_slave_packet_ready=0x00;
- dyn_slave_sending_packet=0x00;
-
- // configure the interrupts
- NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
- // configure the GPIO pins
- /* Configure USART1 Rx as input floating */
- GPIO_InitStructure.GPIO_Pin = PIN_USART3_RX;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
- GPIO_Init(PORT_USART3_RX, &GPIO_InitStructure);
-
- /* Configure USART1 Tx as alternate function push-pull */
- GPIO_InitStructure.GPIO_Pin = PIN_USART3_TX;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
- GPIO_Init(PORT_USART3_TX, &GPIO_InitStructure);
-
- /* configure the nSLEEP input from the FTDI device*/
- GPIO_InitStructure.GPIO_Pin = PIN_nSLEEP;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
- GPIO_Init(PORT_nSLEEP, &GPIO_InitStructure);
-
- USART_DeInit(USART3);
- USART_StructInit(&USART_InitStructure);
- // configure the serial port
- USART_InitStructure.USART_BaudRate = 1000000;
- USART_InitStructure.USART_WordLength = USART_WordLength_8b;
- USART_InitStructure.USART_StopBits = USART_StopBits_1;
- USART_InitStructure.USART_Parity = USART_Parity_No;
- USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
- USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
- USART_Init(USART3, &USART_InitStructure);
-
- // enable the interrupts
- USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);
- USART_ITConfig(USART3, USART_IT_ORE, DISABLE);
- USART_ITConfig(USART3, USART_IT_TXE, ENABLE);
- USART_ITConfig(USART3, USART_IT_TC, DISABLE);
- USART_ITConfig(USART3, USART_IT_CTS, DISABLE);
- USART_ITConfig(USART3, USART_IT_LBD, DISABLE);
- USART_ITConfig(USART3, USART_IT_IDLE, DISABLE);
- USART_ITConfig(USART3, USART_IT_PE, DISABLE);
- USART_ITConfig(USART3, USART_IT_ERR, DISABLE);
-
- /* Enable the USART3 */
- USART_Cmd(USART3, ENABLE);
-}
-
-uint8_t dyn_slave_pc_present(void)
-{
- return GPIO_ReadOutputDataBit(PORT_nSLEEP,PIN_nSLEEP);
-}
-
-void dyn_slave_set_address(uint8_t id)
-{
- dyn_slave_address=id;
-}
-
-uint8_t dyn_slave_get_address(void)
-{
- return dyn_slave_address;
-}
-
-uint8_t dyn_slave_is_packet_ready(void)
-{
- return dyn_slave_packet_ready;
-}
-
-void dyn_slave_get_inst_packet(uint8_t *packet)
-{
- uint8_t i;
-
- for(i=0;i<dyn_slave_rx_num_data;i++)
- packet[i]=dyn_slave_rx_buffer[i];
- dyn_slave_rx_num_data=0x00;
- dyn_slave_packet_ready=0x00;
-}
-
-void dyn_slave_send_status_packet(uint8_t error,uint8_t length, uint8_t *data)
-{
- // wait until the previous transmission has ended (if any)
- while(dyn_slave_sending_packet==0x01);
- // create the status packet
- dyn_init_status_packet(dyn_slave_tx_buffer);
- dyn_set_status_error(dyn_slave_tx_buffer,error);
- dyn_set_id(dyn_slave_tx_buffer,dyn_slave_address);
- dyn_set_status_data(dyn_slave_tx_buffer,length,data);
- dyn_set_checksum(dyn_slave_tx_buffer);
- // start sending the package
- dyn_slave_tx_num_data=dyn_get_length(dyn_slave_tx_buffer)+4;
- dyn_slave_sending_packet=0x01;
- USART_ITConfig(USART3, USART_IT_TXE, ENABLE);
-}
-
-void dyn_slave_resend_status_packet(uint8_t *packet)
-{
- // wait until the previous transmission has ended (if any)
- while(dyn_slave_sending_packet==0x01);
- // create the status packet
- dyn_init_status_packet(dyn_slave_tx_buffer);
- dyn_copy_packet(packet,dyn_slave_tx_buffer);
- // start sending the package
- dyn_slave_tx_num_data=dyn_get_length(dyn_slave_tx_buffer)+4;
- dyn_slave_sending_packet=0x01;
- USART_ITConfig(USART3, USART_IT_TXE, ENABLE);
-}
diff --git a/src/dynamixel_slave_uart_dma.c b/src/dynamixel_slave_uart_dma.c
new file mode 100755
index 0000000000000000000000000000000000000000..268b331fe102a8c55c1ff38f3f6212a5b9637412
--- /dev/null
+++ b/src/dynamixel_slave_uart_dma.c
@@ -0,0 +1,305 @@
+#include "dynamixel_slave_uart_dma.h"
+#include "gpio.h"
+
+#define DYN_SLAVE USART3
+#define DYN_SLAVE_CLK RCC_APB1Periph_USART3
+#define DYN_SLAVE_CLK_INIT RCC_APB1PeriphClockCmd
+#define DYN_SLAVE_IRQn USART3_IRQn
+#define DYN_SLAVE_IRQHandler USART3_IRQHandler
+
+#define DYN_SLAVE_TX_PIN GPIO_Pin_10
+#define DYN_SLAVE_TX_GPIO_PORT GPIOB
+#define DYN_SLAVE_TX_GPIO_CLK RCC_APB2Periph_GPIOB
+#define DYN_SLAVE_TX_SOURCE GPIO_PinSource10
+
+#define DYN_SLAVE_RX_PIN GPIO_Pin_11
+#define DYN_SLAVE_RX_GPIO_PORT GPIOB
+#define DYN_SLAVE_RX_GPIO_CLK RCC_APB2Periph_GPIOB
+#define DYN_SLAVE_RX_SOURCE GPIO_PinSource11
+
+/* DMA configuration */
+#define DYN_SLAVE_DR_ADDRESS ((uint32_t)USART3 + 0x04)
+#define DYN_SLAVE_DMA DMA1
+#define DYN_SLAVE_DMA_CLK RCC_AHBPeriph_DMA1
+
+#define DYN_SLAVE_TX_DMA_CHANNEL DMA1_Channel2
+#define DYN_SLAVE_TX_DMA_FLAG_GLIF DMA1_FLAG_GL2
+#define DYN_SLAVE_TX_DMA_FLAG_TEIF DMA1_FLAG_TE2
+#define DYN_SLAVE_TX_DMA_FLAG_HTIF DMA1_FLAG_HT2
+#define DYN_SLAVE_TX_DMA_FLAG_TCIF DMA1_FLAG_TC2
+
+#define DYN_SLAVE_RX_DMA_CHANNEL DMA1_Channel3
+#define DYN_SLAVE_RX_DMA_FLAG_GLIF DMA1_FLAG_GL3
+#define DYN_SLAVE_RX_DMA_FLAG_TEIF DMA1_FLAG_TE3
+#define DYN_SLAVE_RX_DMA_FLAG_HTIF DMA1_FLAG_HT3
+#define DYN_SLAVE_RX_DMA_FLAG_TCIF DMA1_FLAG_TC3
+
+#define DYN_SLAVE_DMA_TX_IRQn DMA1_Channel2_IRQn
+#define DYN_SLAVE_DMA_RX_IRQn DMA1_Channel3_IRQn
+#define DYN_SLAVE_DMA_TX_IRQHandler DMA1_Channel2_IRQHandler
+#define DYN_SLAVE_DMA_RX_IRQHandler DMA1_Channel3_IRQHandler
+
+#define MAX_BUFFER_LEN 1024
+// private variables
+uint8_t dyn_slave_address;// this module slave address
+uint8_t dyn_slave_return_level;// type of response
+uint8_t dyn_slave_return_delay;// delay in th response
+// input buffer
+uint8_t dyn_slave_rx_buffer[MAX_BUFFER_LEN];
+volatile uint8_t dyn_slave_receiving_header;
+// output buffer
+uint8_t dyn_slave_tx_buffer[MAX_BUFFER_LEN];
+// instruction packet ready flag
+volatile uint8_t dyn_slave_packet_ready;
+// sending status packet flag
+volatile uint8_t dyn_slave_sending_packet;
+// DMA initialization data structures
+DMA_InitTypeDef DYN_SLAVE_DMA_TX_InitStructure;
+DMA_InitTypeDef DYN_SLAVE_DMA_RX_InitStructure;
+
+// private functions
+
+// interrupt handlers
+void DYN_SLAVE_IRQHandler(void)
+{
+ if(USART_GetITStatus(DYN_SLAVE, USART_IT_TC) != RESET)
+ {
+ USART_ClearFlag(DYN_SLAVE,USART_FLAG_TC);
+ USART_ITConfig(DYN_SLAVE, USART_IT_TC, DISABLE);
+ dyn_slave_sending_packet=0x00;
+ // set up the DMA RX transaction
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = 4;
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_slave_rx_buffer;
+ DMA_Init(DYN_SLAVE_RX_DMA_CHANNEL,&DYN_SLAVE_DMA_RX_InitStructure);
+ DMA_ITConfig(DYN_SLAVE_RX_DMA_CHANNEL,DMA_IT_TC,ENABLE);
+ DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,ENABLE);
+ USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
+ dyn_slave_receiving_header=0x01;
+ }
+}
+
+void DYN_SLAVE_DMA_TX_IRQHandler(void)
+{
+ DMA_Cmd(DYN_SLAVE_TX_DMA_CHANNEL,DISABLE);
+ DMA_ClearFlag(DYN_SLAVE_TX_DMA_FLAG_GLIF);
+ DMA_ClearITPendingBit(DYN_SLAVE_TX_DMA_FLAG_GLIF);
+ USART_ITConfig(DYN_SLAVE, USART_IT_TC, ENABLE);
+}
+
+void DYN_SLAVE_DMA_RX_IRQHandler(void)
+{
+ if(dyn_slave_receiving_header==0x01)
+ {
+ DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,DISABLE);
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = dyn_get_length(dyn_slave_rx_buffer);
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&dyn_slave_rx_buffer[4];
+ DMA_Init(DYN_SLAVE_RX_DMA_CHANNEL,&DYN_SLAVE_DMA_RX_InitStructure);
+ DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,ENABLE);
+ USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
+ dyn_slave_receiving_header=0x00;
+ DMA_ClearFlag(DYN_SLAVE_RX_DMA_FLAG_GLIF);
+ DMA_ClearITPendingBit(DYN_SLAVE_RX_DMA_FLAG_GLIF);
+ }
+ else
+ {
+ DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,DISABLE);
+ DMA_ClearFlag(DYN_SLAVE_RX_DMA_FLAG_GLIF);
+ DMA_ClearITPendingBit(DYN_SLAVE_RX_DMA_FLAG_GLIF);
+ DMA_ITConfig(DYN_SLAVE_RX_DMA_CHANNEL,DMA_IT_TC,DISABLE);
+ dyn_slave_packet_ready=0x01;
+ }
+}
+
+// public functions
+void dyn_slave_init(void)
+{
+ uint16_t i;
+ GPIO_InitTypeDef GPIO_InitStructure;
+ NVIC_InitTypeDef NVIC_InitStructure;
+ USART_InitTypeDef USART_InitStructure;
+
+ /* Enable GPIO clock */
+ DYN_SLAVE_CLK_INIT(DYN_SLAVE_CLK, ENABLE);
+ RCC_APB1PeriphClockCmd(DYN_SLAVE_TX_GPIO_CLK | DYN_SLAVE_RX_GPIO_CLK, ENABLE);
+ // configure the GPIO pins
+
+ /* Configure USART Tx and Rx as alternate function push-pull */
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_InitStructure.GPIO_Pin = DYN_SLAVE_TX_PIN;
+ GPIO_Init(DYN_SLAVE_TX_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = DYN_SLAVE_RX_PIN;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(DYN_SLAVE_RX_GPIO_PORT, &GPIO_InitStructure);
+
+ // initialize the buffers
+ for(i=0;i<MAX_BUFFER_LEN;i++)
+ {
+ dyn_slave_rx_buffer[i]=0x00;
+ dyn_slave_tx_buffer[i]=0x00;
+ }
+ // initialize the flags
+ dyn_slave_packet_ready=0x00;
+ dyn_slave_sending_packet=0x00;
+ dyn_slave_receiving_header=0x01;
+
+ USART_DeInit(DYN_SLAVE);
+ USART_StructInit(&USART_InitStructure);
+ // configure the serial port
+ USART_InitStructure.USART_BaudRate = 1000000;
+ USART_InitStructure.USART_WordLength = USART_WordLength_8b;
+ USART_InitStructure.USART_StopBits = USART_StopBits_1;
+ USART_InitStructure.USART_Parity = USART_Parity_No;
+ USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
+ USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
+ USART_Init(DYN_SLAVE, &USART_InitStructure);
+
+ NVIC_InitStructure.NVIC_IRQChannel = DYN_SLAVE_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+ USART_ITConfig(DYN_SLAVE, USART_IT_RXNE | USART_IT_ORE | USART_IT_TXE | USART_IT_CTS | USART_IT_LBD | USART_IT_IDLE | USART_IT_PE | USART_IT_ERR | USART_IT_TC, DISABLE);
+ USART_ClearFlag(DYN_SLAVE,USART_FLAG_RXNE | USART_FLAG_ORE | USART_FLAG_TXE | USART_FLAG_CTS | USART_FLAG_LBD | USART_FLAG_IDLE | USART_FLAG_PE | USART_FLAG_TC);
+ USART_ClearITPendingBit(DYN_SLAVE, USART_FLAG_RXNE | USART_FLAG_ORE | USART_FLAG_TXE | USART_FLAG_CTS | USART_FLAG_LBD | USART_FLAG_IDLE | USART_FLAG_PE | USART_FLAG_TC);
+ /* Enable the DYN_SLAVE3 */
+ USART_Cmd(DYN_SLAVE, ENABLE);
+
+ // configure the DMA channels
+ /* Configure TX DMA */
+ RCC_AHBPeriphClockCmd(DYN_SLAVE_DMA_CLK, ENABLE);
+ DMA_DeInit(DYN_SLAVE_TX_DMA_CHANNEL);
+ DYN_SLAVE_DMA_TX_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+ DYN_SLAVE_DMA_TX_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+ DYN_SLAVE_DMA_TX_InitStructure.DMA_Mode = DMA_Mode_Normal;
+ DYN_SLAVE_DMA_TX_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) (&(DYN_SLAVE->DR));
+ DYN_SLAVE_DMA_TX_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+ DYN_SLAVE_DMA_TX_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+ DYN_SLAVE_DMA_TX_InitStructure.DMA_Priority = DMA_Priority_High;
+ DYN_SLAVE_DMA_TX_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
+ DYN_SLAVE_DMA_TX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_slave_tx_buffer;
+ DYN_SLAVE_DMA_TX_InitStructure.DMA_M2M = DMA_M2M_Disable;
+ DMA_Init(DYN_SLAVE_TX_DMA_CHANNEL,&DYN_SLAVE_DMA_TX_InitStructure);
+ /* initialize DMA interrupts */
+ NVIC_InitStructure.NVIC_IRQChannel = DYN_SLAVE_DMA_TX_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+ DMA_ITConfig(DYN_SLAVE_TX_DMA_CHANNEL,DMA_IT_TC,ENABLE);
+ DMA_ITConfig(DYN_SLAVE_TX_DMA_CHANNEL,DMA_IT_HT | DMA_IT_TE,DISABLE);
+
+ /* Configure RX DMA */
+ DMA_DeInit(DYN_SLAVE_RX_DMA_CHANNEL);
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = 4;// transfer the first 3 bytes
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_Mode = DMA_Mode_Normal;
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) (&(DYN_SLAVE->DR));
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_Priority = DMA_Priority_High;
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_slave_rx_buffer;
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_M2M = DMA_M2M_Disable;
+ DMA_Init(DYN_SLAVE_RX_DMA_CHANNEL,&DYN_SLAVE_DMA_RX_InitStructure);
+ /* initialize DMA interrupts */
+ NVIC_InitStructure.NVIC_IRQChannel = DYN_SLAVE_DMA_RX_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+ DMA_ITConfig(DYN_SLAVE_RX_DMA_CHANNEL,DMA_IT_TC,ENABLE);
+ DMA_ITConfig(DYN_SLAVE_RX_DMA_CHANNEL,DMA_IT_HT | DMA_IT_TE,DISABLE);
+
+ DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,ENABLE);
+ USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
+}
+
+inline void dyn_slave_set_address(uint8_t id)
+{
+ dyn_slave_address=id;
+}
+
+inline uint8_t dyn_slave_get_address(void)
+{
+ return dyn_slave_address;
+}
+
+inline void dyn_slave_set_return_delay(uint8_t delay)
+{
+ dyn_slave_return_delay=delay;
+}
+
+inline void dyn_slave_set_return_level(uint8_t level)
+{
+ dyn_slave_return_level=level;
+}
+
+inline uint8_t dyn_slave_get_return_level(void)
+{
+ return dyn_slave_return_level;
+}
+
+uint8_t dyn_slave_is_packet_ready(void)
+{
+ return dyn_slave_packet_ready;
+}
+
+void dyn_slave_get_inst_packet(uint8_t *packet)
+{
+ uint8_t i;
+
+ for(i=0;i<dyn_get_length(dyn_slave_rx_buffer)+4;i++)
+ packet[i]=dyn_slave_rx_buffer[i];
+ dyn_slave_packet_ready=0x00;
+}
+
+void dyn_slave_send_status_packet(uint8_t error,uint8_t length, uint8_t *data)
+{
+ // wait until the previous transmission has ended (if any)
+ while(dyn_slave_sending_packet==0x01);
+ // create the status packet
+ dyn_init_status_packet(dyn_slave_tx_buffer);
+ dyn_set_status_error(dyn_slave_tx_buffer,error);
+ dyn_set_id(dyn_slave_tx_buffer,dyn_slave_address);
+ dyn_set_status_data(dyn_slave_tx_buffer,length,data);
+ dyn_set_checksum(dyn_slave_tx_buffer);
+ // set the DMA transfer
+ DMA_SetCurrDataCounter(DYN_SLAVE_TX_DMA_CHANNEL,dyn_get_length(dyn_slave_tx_buffer)+4);
+ DMA_Cmd(DYN_SLAVE_TX_DMA_CHANNEL,ENABLE);
+ USART_ClearFlag(DYN_SLAVE,USART_FLAG_TC);
+ USART_DMACmd(DYN_SLAVE, USART_DMAReq_Tx, ENABLE);
+ dyn_slave_sending_packet=0x01;
+}
+
+void dyn_slave_resend_status_packet(uint8_t *packet)
+{
+ // wait until the previous transmission has ended (if any)
+ while(dyn_slave_sending_packet==0x01);
+ // create the status packet
+ dyn_init_status_packet(dyn_slave_tx_buffer);
+ dyn_copy_packet(packet,dyn_slave_tx_buffer);
+ // set the DMA transfer
+ DMA_SetCurrDataCounter(DYN_SLAVE_TX_DMA_CHANNEL,dyn_get_length(dyn_slave_tx_buffer)+4);
+ DMA_Cmd(DYN_SLAVE_TX_DMA_CHANNEL,ENABLE);
+ USART_ClearFlag(DYN_SLAVE,USART_FLAG_TC);
+ USART_DMACmd(DYN_SLAVE, USART_DMAReq_Tx, ENABLE);
+ dyn_slave_sending_packet=0x01;
+}
+
+void dyn_slave_reset(void)
+{
+ dyn_slave_sending_packet=0x00;
+ // set up the DMA RX transaction
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = 4;
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_slave_rx_buffer;
+ DMA_Init(DYN_SLAVE_RX_DMA_CHANNEL,&DYN_SLAVE_DMA_RX_InitStructure);
+ DMA_ITConfig(DYN_SLAVE_RX_DMA_CHANNEL,DMA_IT_TC,ENABLE);
+ DMA_Cmd(DYN_SLAVE_RX_DMA_CHANNEL,ENABLE);
+ USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
+ dyn_slave_receiving_header=0x01;
+}
diff --git a/src/eeprom.c b/src/eeprom.c
index 89b88f3e57f5e6b61518848e9a20609dd11c9ecd..96dff4b16d429620d008653df5e6c0f946ffb615 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -1,140 +1,641 @@
-#include "eeprom.h"
-
-#define EEPROM_START_ADDRESS ((uint32_t)0x08003800)
-#define EEPROM_SIZE 0x3FF
-#define DEFAULT_DEVICE_MODEL 0x7300
-#define DEFAULT_FIRMWARE_VERSION 0x13
-#define DEFAULT_DEVICE_ID 0xC8
-#define DEFAULT_BAUDRATE 0x01
-#define DEFAULT_RETURN_DELAY 0x00
-#define DEFAULT_RETURN_LEVEL 0x02
-
-typedef enum{
- DEVICE_MODEL_OFFSET = ((uint32_t)0x000000000),
- FIRMWARE_VERSION_OFFSET = ((uint32_t)0x000000002),
- DEVICE_ID_OFFSET = ((uint32_t)0x000000003),
- BAUDRATE_OFFSET = ((uint32_t)0x000000004),
- RETURN_DELAY_OFFSET = ((uint32_t)0x000000005),
- RETURN_LEVEL_OFFSET = ((uint32_t)0x000000010)
-}TEEPROMReg;
-
-// EEPROM memory map
-uint16_t eeprom_data[EEPROM_SIZE] __attribute__ ((section (".eeprom")))={DEFAULT_DEVICE_MODEL&0xFF,// model number LSB
- DEFAULT_DEVICE_MODEL>>8,// model number MSB
- DEFAULT_FIRMWARE_VERSION,// firmware version
- DEFAULT_DEVICE_ID,// default device id
- DEFAULT_BAUDRATE,// default baudrate
- DEFAULT_RETURN_DELAY,// return delay time
- 0x0000,// not defined
- 0x0000,// not defined
- 0x0000,// not defined
- 0x0000,// not defined
- 0x0000,// not defined
- 0x0000,// not defined
- 0x0000,// not defined
- 0x0000,// not defined
- 0x0000,// not defined
- 0x0000,// not defined
- DEFAULT_RETURN_LEVEL,// return level
- 0x0000,// not defined
- 0x0000,// not defined
- 0x0000,// not defined
- 0x0000,// not defined
- 0x0000,// not defined
- 0x0000,// not defined
- 0x0000,};// not defined
-
-// EEPROM memory write access enable
-const uint8_t eeprom_write_access[EEPROM_SIZE]={0x00,0x00,0x00,0x01,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
- 0x00,0x00,0x01,0x00,};// all others disabled
-
-// private functions
-uint16_t eeprom_read(uint32_t offset)
-{
- uint16_t *adr;
-
- adr = (uint16_t *)(EEPROM_START_ADDRESS + (offset<<1));
- return *adr;
-}
-
-// public functions
-// read operations
-uint16_t eeprom_read_device_model(void)
-{
- return eeprom_read(DEVICE_MODEL_OFFSET);
-}
-
-uint8_t eeprom_read_firmware_version(void)
-{
- return eeprom_read(FIRMWARE_VERSION_OFFSET);
-}
-
-uint8_t eeprom_read_device_id(void)
-{
- return eeprom_read(DEVICE_ID_OFFSET);
-}
-
-uint8_t eeprom_read_baudrate(void)
-{
- return eeprom_read(BAUDRATE_OFFSET);
-}
-
-uint8_t eeprom_read_return_delay(void)
-{
- return eeprom_read(RETURN_LEVEL_OFFSET);
-}
-
-uint8_t eeprom_read_return_level(void)
-{
- return eeprom_read(RETURN_LEVEL_OFFSET);
-}
-
-uint8_t eeprom_read_table(uint32_t start_offset, int16_t length,uint8_t *data)
-{
- uint8_t i;
-
- if((start_offset+length)>EEPROM_DATA_SIZE)
- return EEPROM_BAD_ADDRESS;
- else
- {
- for(i=0;i<length;i++)
- data[i]=eeprom_read(start_offset+i);
- return EEPROM_SUCCESS;
- }
-}
-
-// write operations
-uint8_t eeprom_write_table(uint32_t start_offset, int16_t length,uint8_t *data)
-{
- volatile FLASH_Status FLASHStatus;
- uint32_t adr;
- uint8_t cnt,i;
- uint16_t buffer[EEPROM_DATA_SIZE];
-
- if((start_offset+length) < EEPROM_DATA_SIZE)
- {
- // read the current contents of the FLASH sectors
- for(cnt=0; cnt<EEPROM_DATA_SIZE; cnt++)
- buffer[cnt] = eeprom_read(cnt);
- // change the new data
- for(cnt=start_offset,i=0; cnt<(start_offset+length); cnt++,i++)
- if(eeprom_write_access[start_offset+length])// if the address has write access
- buffer[cnt] = data[i];
-
- FLASH_Unlock();
- /* Clear All pending flags */
- FLASH_ClearFlag(FLASH_FLAG_BSY | FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
-
- /* Erase the FLASH pages */
- FLASHStatus = FLASH_ErasePage(EEPROM_START_ADDRESS);
- adr = EEPROM_START_ADDRESS;
- for(cnt=0; cnt<EEPROM_DATA_SIZE; cnt++)
- {
- FLASHStatus = FLASH_ProgramHalfWord(adr, buffer[cnt]);
- adr += 2;
- }
- FLASH_Lock();
- }
-
- return EEPROM_SUCCESS;
-}
+/**
+ ******************************************************************************
+ * @file EEPROM_Emulation/src/eeprom.c
+ * @author MCD Application Team
+ * @version V1.0.0
+ * @date 10-October-2011
+ * @brief This file provides all the EEPROM emulation firmware functions.
+ ******************************************************************************
+ * @attention
+ *
+ * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+ * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
+ * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
+ * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
+ * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
+ * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+ *
+ * <h2><center>© COPYRIGHT 2011 STMicroelectronics</center></h2>
+ ******************************************************************************
+ */
+
+/** @addtogroup EEPROM_Emulation
+ * @{
+ */
+
+/* Includes ------------------------------------------------------------------*/
+#include "eeprom.h"
+
+/* Private typedef -----------------------------------------------------------*/
+/* Private define ------------------------------------------------------------*/
+#define EEPROM_SIZE 0x09
+#define DEFAULT_DEVICE_MODEL 0x7300
+#define DEFAULT_FIRMWARE_VERSION 0x13
+#define DEFAULT_DEVICE_ID 0xC0
+#define DEFAULT_BAUDRATE 0x01
+#define DEFAULT_RETURN_DELAY 0x00
+#define DEFAULT_MM_PERIOD 0x01FF
+#define DEFAULT_RETURN_LEVEL 0x02
+/* Private macro -------------------------------------------------------------*/
+/* Private variables ---------------------------------------------------------*/
+
+/* Global variable used to store variable value in read sequence */
+uint16_t DataVar = 0;
+
+// EEPROM memory map
+uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
+ 0xFFFF,
+ DEFAULT_DEVICE_MODEL&0xFF,// model number LSB
+ DEVICE_MODEL_OFFSET,
+ DEFAULT_DEVICE_MODEL>>8,// model number MSB
+ DEVICE_MODEL_OFFSET+1,
+ DEFAULT_FIRMWARE_VERSION,// firmware version
+ FIRMWARE_VERSION_OFFSET,
+ DEFAULT_DEVICE_ID,// default device id
+ DEVICE_ID_OFFSET,
+ DEFAULT_BAUDRATE,// default baudrate
+ BAUDRATE_OFFSET,
+ DEFAULT_RETURN_DELAY,// return delay time
+ RETURN_DELAY_OFFSET,
+ DEFAULT_MM_PERIOD&0xFF,
+ MM_PERIOD_OFFSET,
+ DEFAULT_MM_PERIOD>>8,
+ MM_PERIOD_OFFSET+1,
+ DEFAULT_RETURN_LEVEL,
+ RETURN_LEVEL_OFFSET};// return level
+
+/* Private function prototypes -----------------------------------------------*/
+/* Private functions ---------------------------------------------------------*/
+static FLASH_Status EE_Format(void);
+static uint16_t EE_FindValidPage(uint8_t Operation);
+static uint16_t EE_VerifyPageFullWriteVariable(uint16_t VirtAddress, uint16_t Data);
+static uint16_t EE_PageTransfer(uint16_t VirtAddress, uint16_t Data);
+
+/**
+ * @brief Restore the pages to a known good state in case of page's status
+ * corruption after a power loss.
+ * @param None.
+ * @retval - Flash error code: on write Flash error
+ * - FLASH_COMPLETE: on success
+ */
+uint16_t EE_Init(void)
+{
+ uint16_t PageStatus0 = 6, PageStatus1 = 6;
+ uint16_t VarIdx = 0;
+ uint16_t EepromStatus = 0, ReadStatus = 0;
+ int16_t x = -1;
+ uint16_t FlashStatus;
+
+ FLASH_Unlock();
+ /* Get Page0 status */
+ PageStatus0 = (*(__IO uint16_t*)PAGE0_BASE_ADDRESS);
+ /* Get Page1 status */
+ PageStatus1 = (*(__IO uint16_t*)PAGE1_BASE_ADDRESS);
+
+ /* Check for invalid header states and repair if necessary */
+ switch (PageStatus0)
+ {
+ case ERASED:
+ if (PageStatus1 == VALID_PAGE) /* Page0 erased, Page1 valid */
+ {
+ /* Erase Page0 */
+ FlashStatus = FLASH_ErasePage(PAGE0_BASE_ADDRESS);
+ /* If erase operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+ }
+ else if (PageStatus1 == RECEIVE_DATA) /* Page0 erased, Page1 receive */
+ {
+ /* Erase Page0 */
+ FlashStatus = FLASH_ErasePage(PAGE0_BASE_ADDRESS);
+ /* If erase operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+ /* Mark Page1 as valid */
+ FlashStatus = FLASH_ProgramHalfWord(PAGE1_BASE_ADDRESS, VALID_PAGE);
+ /* If program operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+ }
+ else /* First EEPROM access (Page0&1 are erased) or invalid state -> format EEPROM */
+ {
+ /* Erase both Page0 and Page1 and set Page0 as valid page */
+ FlashStatus = EE_Format();
+ /* If erase/program operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+ }
+ break;
+
+ case RECEIVE_DATA:
+ if (PageStatus1 == VALID_PAGE) /* Page0 receive, Page1 valid */
+ {
+ /* Transfer data from Page1 to Page0 */
+ for (VarIdx = 0; VarIdx < EEPROM_SIZE; VarIdx++)
+ {
+ if (( *(__IO uint16_t*)(PAGE0_BASE_ADDRESS + 6)) == eeprom_data[VarIdx])
+ {
+ x = VarIdx;
+ }
+ if (VarIdx != x)
+ {
+ /* Read the last variables' updates */
+ ReadStatus = EE_ReadVariable(eeprom_data[VarIdx], &DataVar);
+ /* In case variable corresponding to the virtual address was found */
+ if (ReadStatus != 0x1)
+ {
+ /* Transfer the variable to the Page0 */
+ EepromStatus = EE_VerifyPageFullWriteVariable(eeprom_data[VarIdx], DataVar);
+ /* If program operation was failed, a Flash error code is returned */
+ if (EepromStatus != FLASH_COMPLETE)
+ {
+ return EepromStatus;
+ }
+ }
+ }
+ }
+ /* Mark Page0 as valid */
+ FlashStatus = FLASH_ProgramHalfWord(PAGE0_BASE_ADDRESS, VALID_PAGE);
+ /* If program operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+ /* Erase Page1 */
+ FlashStatus = FLASH_ErasePage(PAGE1_BASE_ADDRESS);
+ /* If erase operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+ }
+ else if (PageStatus1 == ERASED) /* Page0 receive, Page1 erased */
+ {
+ /* Erase Page1 */
+ FlashStatus = FLASH_ErasePage(PAGE1_BASE_ADDRESS);
+ /* If erase operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+ /* Mark Page0 as valid */
+ FlashStatus = FLASH_ProgramHalfWord(PAGE0_BASE_ADDRESS, VALID_PAGE);
+ /* If program operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+ }
+ else /* Invalid state -> format eeprom */
+ {
+ /* Erase both Page0 and Page1 and set Page0 as valid page */
+ FlashStatus = EE_Format();
+ /* If erase/program operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+ }
+ break;
+
+ case VALID_PAGE:
+ if (PageStatus1 == VALID_PAGE) /* Invalid state -> format eeprom */
+ {
+ /* Erase both Page0 and Page1 and set Page0 as valid page */
+ FlashStatus = EE_Format();
+ /* If erase/program operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+ }
+ else if (PageStatus1 == ERASED) /* Page0 valid, Page1 erased */
+ {
+ /* Erase Page1 */
+ FlashStatus = FLASH_ErasePage(PAGE1_BASE_ADDRESS);
+ /* If erase operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+ }
+ else /* Page0 valid, Page1 receive */
+ {
+ /* Transfer data from Page0 to Page1 */
+ for (VarIdx = 0; VarIdx < EEPROM_SIZE; VarIdx++)
+ {
+ if ((*(__IO uint16_t*)(PAGE1_BASE_ADDRESS + 6)) == eeprom_data[VarIdx])
+ {
+ x = VarIdx;
+ }
+ if (VarIdx != x)
+ {
+ /* Read the last variables' updates */
+ ReadStatus = EE_ReadVariable(eeprom_data[VarIdx], &DataVar);
+ /* In case variable corresponding to the virtual address was found */
+ if (ReadStatus != 0x1)
+ {
+ /* Transfer the variable to the Page1 */
+ EepromStatus = EE_VerifyPageFullWriteVariable(eeprom_data[VarIdx], DataVar);
+ /* If program operation was failed, a Flash error code is returned */
+ if (EepromStatus != FLASH_COMPLETE)
+ {
+ return EepromStatus;
+ }
+ }
+ }
+ }
+ /* Mark Page1 as valid */
+ FlashStatus = FLASH_ProgramHalfWord(PAGE1_BASE_ADDRESS, VALID_PAGE);
+ /* If program operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+ /* Erase Page0 */
+ FlashStatus = FLASH_ErasePage(PAGE0_BASE_ADDRESS);
+ /* If erase operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+ }
+ break;
+
+ default: /* Any other state -> format eeprom */
+ /* Erase both Page0 and Page1 and set Page0 as valid page */
+ FlashStatus = EE_Format();
+ /* If erase/program operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+ break;
+ }
+
+ return FLASH_COMPLETE;
+}
+
+/**
+ * @brief Returns the last stored variable data, if found, which correspond to
+ * the passed virtual address
+ * @param VirtAddress: Variable virtual address
+ * @param Data: Global variable contains the read variable value
+ * @retval Success or error status:
+ * - 0: if variable was found
+ * - 1: if the variable was not found
+ * - NO_VALID_PAGE: if no valid page was found.
+ */
+uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data)
+{
+ uint16_t ValidPage = PAGE0;
+ uint16_t AddressValue = 0x5555, ReadStatus = 1;
+ uint32_t Address = EEPROM_START_ADDRESS, PageStartAddress = EEPROM_START_ADDRESS;
+
+ /* Get active Page for read operation */
+ ValidPage = EE_FindValidPage(READ_FROM_VALID_PAGE);
+
+ /* Check if there is no valid page */
+ if (ValidPage == NO_VALID_PAGE)
+ {
+ return NO_VALID_PAGE;
+ }
+
+ /* Get the valid Page start Address */
+ PageStartAddress = (uint32_t)(EEPROM_START_ADDRESS + (uint32_t)(ValidPage * PAGE_SIZE));
+
+ /* Get the valid Page end Address */
+ Address = (uint32_t)((EEPROM_START_ADDRESS - 2) + (uint32_t)((1 + ValidPage) * PAGE_SIZE));
+
+ /* Check each active page address starting from end */
+ while (Address > (PageStartAddress + 2))
+ {
+ /* Get the current location content to be compared with virtual address */
+ AddressValue = (*(__IO uint16_t*)Address);
+
+ /* Compare the read address with the virtual address */
+ if (AddressValue == VirtAddress)
+ {
+ /* Get content of Address-2 which is variable value */
+ *Data = (*(__IO uint16_t*)(Address - 2));
+
+ /* In case variable value is read, reset ReadStatus flag */
+ ReadStatus = 0;
+
+ break;
+ }
+ else
+ {
+ /* Next address location */
+ Address = Address - 4;
+ }
+ }
+
+ /* Return ReadStatus value: (0: variable exist, 1: variable doesn't exist) */
+ return ReadStatus;
+}
+
+/**
+ * @brief Writes/upadtes variable data in EEPROM.
+ * @param VirtAddress: Variable virtual address
+ * @param Data: 16 bit data to be written
+ * @retval Success or error status:
+ * - FLASH_COMPLETE: on success
+ * - PAGE_FULL: if valid page is full
+ * - NO_VALID_PAGE: if no valid page was found
+ * - Flash error code: on write Flash error
+ */
+uint16_t EE_WriteVariable(uint16_t VirtAddress, uint16_t Data)
+{
+ uint16_t Status = 0;
+
+ /* Write the variable virtual address and value in the EEPROM */
+ Status = EE_VerifyPageFullWriteVariable(VirtAddress, Data);
+
+ /* In case the EEPROM active page is full */
+ if (Status == PAGE_FULL)
+ {
+ /* Perform Page transfer */
+ Status = EE_PageTransfer(VirtAddress, Data);
+ }
+
+ /* Return last operation status */
+ return Status;
+}
+
+/**
+ * @brief Erases PAGE and PAGE1 and writes VALID_PAGE header to PAGE
+ * @param None
+ * @retval Status of the last operation (Flash write or erase) done during
+ * EEPROM formating
+ */
+static FLASH_Status EE_Format(void)
+{
+ FLASH_Status FlashStatus = FLASH_COMPLETE;
+
+ /* Erase Page0 */
+ FlashStatus = FLASH_ErasePage(PAGE0_BASE_ADDRESS);
+
+ /* If erase operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+
+ /* Set Page0 as valid page: Write VALID_PAGE at Page0 base address */
+ FlashStatus = FLASH_ProgramHalfWord(PAGE0_BASE_ADDRESS, VALID_PAGE);
+
+ /* If program operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+
+ /* Erase Page1 */
+ FlashStatus = FLASH_ErasePage(PAGE1_BASE_ADDRESS);
+
+ /* Return Page1 erase operation status */
+ return FlashStatus;
+}
+
+/**
+ * @brief Find valid Page for write or read operation
+ * @param Operation: operation to achieve on the valid page.
+ * This parameter can be one of the following values:
+ * @arg READ_FROM_VALID_PAGE: read operation from valid page
+ * @arg WRITE_IN_VALID_PAGE: write operation from valid page
+ * @retval Valid page number (PAGE or PAGE1) or NO_VALID_PAGE in case
+ * of no valid page was found
+ */
+static uint16_t EE_FindValidPage(uint8_t Operation)
+{
+ uint16_t PageStatus0 = 6, PageStatus1 = 6;
+
+ /* Get Page0 actual status */
+ PageStatus0 = (*(__IO uint16_t*)PAGE0_BASE_ADDRESS);
+
+ /* Get Page1 actual status */
+ PageStatus1 = (*(__IO uint16_t*)PAGE1_BASE_ADDRESS);
+
+ /* Write or read operation */
+ switch (Operation)
+ {
+ case WRITE_IN_VALID_PAGE: /* ---- Write operation ---- */
+ if (PageStatus1 == VALID_PAGE)
+ {
+ /* Page0 receiving data */
+ if (PageStatus0 == RECEIVE_DATA)
+ {
+ return PAGE0; /* Page0 valid */
+ }
+ else
+ {
+ return PAGE1; /* Page1 valid */
+ }
+ }
+ else if (PageStatus0 == VALID_PAGE)
+ {
+ /* Page1 receiving data */
+ if (PageStatus1 == RECEIVE_DATA)
+ {
+ return PAGE1; /* Page1 valid */
+ }
+ else
+ {
+ return PAGE0; /* Page0 valid */
+ }
+ }
+ else
+ {
+ return NO_VALID_PAGE; /* No valid Page */
+ }
+
+ case READ_FROM_VALID_PAGE: /* ---- Read operation ---- */
+ if (PageStatus0 == VALID_PAGE)
+ {
+ return PAGE0; /* Page0 valid */
+ }
+ else if (PageStatus1 == VALID_PAGE)
+ {
+ return PAGE1; /* Page1 valid */
+ }
+ else
+ {
+ return NO_VALID_PAGE ; /* No valid Page */
+ }
+
+ default:
+ return PAGE0; /* Page0 valid */
+ }
+}
+
+/**
+ * @brief Verify if active page is full and Writes variable in EEPROM.
+ * @param VirtAddress: 16 bit virtual address of the variable
+ * @param Data: 16 bit data to be written as variable value
+ * @retval Success or error status:
+ * - FLASH_COMPLETE: on success
+ * - PAGE_FULL: if valid page is full
+ * - NO_VALID_PAGE: if no valid page was found
+ * - Flash error code: on write Flash error
+ */
+static uint16_t EE_VerifyPageFullWriteVariable(uint16_t VirtAddress, uint16_t Data)
+{
+ FLASH_Status FlashStatus = FLASH_COMPLETE;
+ uint16_t ValidPage = PAGE0;
+ uint32_t Address = EEPROM_START_ADDRESS, PageEndAddress = EEPROM_START_ADDRESS+PAGE_SIZE;
+
+ /* Get valid Page for write operation */
+ ValidPage = EE_FindValidPage(WRITE_IN_VALID_PAGE);
+
+ /* Check if there is no valid page */
+ if (ValidPage == NO_VALID_PAGE)
+ {
+ return NO_VALID_PAGE;
+ }
+
+ /* Get the valid Page start Address */
+ Address = (uint32_t)(EEPROM_START_ADDRESS + (uint32_t)(ValidPage * PAGE_SIZE));
+
+ /* Get the valid Page end Address */
+ PageEndAddress = (uint32_t)((EEPROM_START_ADDRESS - 2) + (uint32_t)((1 + ValidPage) * PAGE_SIZE));
+
+ /* Check each active page address starting from begining */
+ while (Address < PageEndAddress)
+ {
+ /* Verify if Address and Address+2 contents are 0xFFFFFFFF */
+ if ((*(__IO uint32_t*)Address) == 0xFFFFFFFF)
+ {
+ /* Set variable data */
+ FlashStatus = FLASH_ProgramHalfWord(Address, Data);
+ /* If program operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+ /* Set variable virtual address */
+ FlashStatus = FLASH_ProgramHalfWord(Address + 2, VirtAddress);
+ /* Return program operation status */
+ return FlashStatus;
+ }
+ else
+ {
+ /* Next address location */
+ Address = Address + 4;
+ }
+ }
+ /* Return PAGE_FULL in case the valid page is full */
+ return PAGE_FULL;
+}
+
+/**
+ * @brief Transfers last updated variables data from the full Page to
+ * an empty one.
+ * @param VirtAddress: 16 bit virtual address of the variable
+ * @param Data: 16 bit data to be written as variable value
+ * @retval Success or error status:
+ * - FLASH_COMPLETE: on success
+ * - PAGE_FULL: if valid page is full
+ * - NO_VALID_PAGE: if no valid page was found
+ * - Flash error code: on write Flash error
+ */
+static uint16_t EE_PageTransfer(uint16_t VirtAddress, uint16_t Data)
+{
+ FLASH_Status FlashStatus = FLASH_COMPLETE;
+ uint32_t NewPageAddress = EEPROM_START_ADDRESS;
+ uint32_t OldPageId=0;
+ uint16_t ValidPage = PAGE0, VarIdx = 0;
+ uint16_t EepromStatus = 0, ReadStatus = 0;
+
+ /* Get active Page for read operation */
+ ValidPage = EE_FindValidPage(READ_FROM_VALID_PAGE);
+
+ if (ValidPage == PAGE1) /* Page1 valid */
+ {
+ /* New page address where variable will be moved to */
+ NewPageAddress = PAGE0_BASE_ADDRESS;
+
+ /* Old page ID where variable will be taken from */
+ OldPageId = PAGE1_BASE_ADDRESS;
+ }
+ else if (ValidPage == PAGE0) /* Page0 valid */
+ {
+ /* New page address where variable will be moved to */
+ NewPageAddress = PAGE1_BASE_ADDRESS;
+
+ /* Old page ID where variable will be taken from */
+ OldPageId = PAGE0_BASE_ADDRESS;
+ }
+ else
+ {
+ return NO_VALID_PAGE; /* No valid Page */
+ }
+ /* Set the new Page status to RECEIVE_DATA status */
+ FlashStatus = FLASH_ProgramHalfWord(NewPageAddress, RECEIVE_DATA);
+ /* If program operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+ /* Write the variable passed as parameter in the new active page */
+ EepromStatus = EE_VerifyPageFullWriteVariable(VirtAddress, Data);
+ /* If program operation was failed, a Flash error code is returned */
+ if (EepromStatus != FLASH_COMPLETE)
+ {
+ return EepromStatus;
+ }
+
+ /* Transfer process: transfer variables from old to the new active page */
+ for (VarIdx = 0; VarIdx < EEPROM_SIZE; VarIdx++)
+ {
+ if (eeprom_data[VarIdx] != VirtAddress) /* Check each variable except the one passed as parameter */
+ {
+ /* Read the other last variable updates */
+ ReadStatus = EE_ReadVariable(eeprom_data[VarIdx], &DataVar);
+ /* In case variable corresponding to the virtual address was found */
+ if (ReadStatus != 0x1)
+ {
+ /* Transfer the variable to the new active page */
+ EepromStatus = EE_VerifyPageFullWriteVariable(eeprom_data[VarIdx], DataVar);
+ /* If program operation was failed, a Flash error code is returned */
+ if (EepromStatus != FLASH_COMPLETE)
+ {
+ return EepromStatus;
+ }
+ }
+ }
+ }
+
+ /* Erase the old Page: Set old Page status to ERASED status */
+ FlashStatus = FLASH_ErasePage(OldPageId);
+ /* If erase operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+
+ /* Set new Page status to VALID_PAGE status */
+ FlashStatus = FLASH_ProgramHalfWord(NewPageAddress, VALID_PAGE);
+ /* If program operation was failed, a Flash error code is returned */
+ if (FlashStatus != FLASH_COMPLETE)
+ {
+ return FlashStatus;
+ }
+
+ /* Return last operation flash status */
+ return FlashStatus;
+}
+
+/**
+ * @}
+ */
+
+/******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/
diff --git a/src/gpio.c b/src/gpio.c
index 8557dfe8db01cc40671f869402392b24b9cd26b8..0c1c27795bbc24df2d6c4fa6106a1b93d2a3396a 100755
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -1,511 +1,764 @@
#include "gpio.h"
-#include "ram.h"
-
-//LED macros
-#define PIN_LED2 GPIO_Pin_15
-#define PIN_LED3 GPIO_Pin_12
-#define PIN_LED4 GPIO_Pin_6
-#define PIN_RX_LED GPIO_Pin_14
-#define PIN_TX_LED GPIO_Pin_13
-#define PORT_LED2 GPIOC
-#define PORT_LED3 GPIOB
-#define PORT_LED4 GPIOC
-#define PORT_RX_LED GPIOC
-#define PORT_TX_LED GPIOC
-
-// RGB LED macros
-#define PIN_HEAD_R_LED GPIO_Pin_7
-#define PIN_HEAD_G_LED GPIO_Pin_8
-#define PIN_HEAD_B_LED GPIO_Pin_9
-#define PIN_EYE_R_LED GPIO_Pin_8
-#define PIN_EYE_G_LED GPIO_Pin_11
-#define PIN_EYE_B_LED GPIO_Pin_12
-#define PORT_HEAD_LEDS GPIOC
-#define PORT_EYE_LEDS GPIOA
-
-// PUSHBUTTON macros
-#define PIN_SW_MODE GPIO_Pin_14
-#define PIN_SW_START GPIO_Pin_15
-#define PORT_PUSHBUTTONS GPIOA
-
-// timer 2 period values
-const __IO uint16_t color_gen_period=10; // 10KHz
-const __IO uint16_t blink_period=100; // 1kHz
-const __IO uint16_t pushbuttons_period=10000; // 10 Hz
+
+#define LED_TX_GPIO_CLK RCC_APB2Periph_GPIOC
+#define LED_TX_PIN GPIO_Pin_13
+#define LED_TX_GPIO_PORT GPIOC
+#define LED_TX_SOURCE GPIO_PinSource13
+
+#define LED_RX_GPIO_CLK RCC_APB2Periph_GPIOC
+#define LED_RX_PIN GPIO_Pin_14
+#define LED_RX_GPIO_PORT GPIOC
+#define LED_RX_SOURCE GPIO_PinSource14
+
+#define LED_2_GPIO_CLK RCC_APB2Periph_GPIOC
+#define LED_2_PIN GPIO_Pin_15
+#define LED_2_GPIO_PORT GPIOC
+#define LED_2_SOURCE GPIO_PinSource15
+
+#define LED_3_GPIO_CLK RCC_APB2Periph_GPIOC
+#define LED_3_PIN GPIO_Pin_6
+#define LED_3_GPIO_PORT GPIOC
+#define LED_3_SOURCE GPIO_PinSource6
+
+#define LED_4_GPIO_CLK RCC_APB2Periph_GPIOB
+#define LED_4_PIN GPIO_Pin_12
+#define LED_4_GPIO_PORT GPIOB
+#define LED_4_SOURCE GPIO_PinSource12
+
+#define LED_5_R_GPIO_CLK RCC_APB2Periph_GPIOC
+#define LED_5_R_PIN GPIO_Pin_7
+#define LED_5_R_GPIO_PORT GPIOC
+#define LED_5_R_SOURCE GPIO_PinSource7
+
+#define LED_5_G_GPIO_CLK RCC_APB2Periph_GPIOC
+#define LED_5_G_PIN GPIO_Pin_8
+#define LED_5_G_GPIO_PORT GPIOC
+#define LED_5_G_SOURCE GPIO_PinSource8
+
+#define LED_5_B_GPIO_CLK RCC_APB2Periph_GPIOC
+#define LED_5_B_PIN GPIO_Pin_9
+#define LED_5_B_GPIO_PORT GPIOC
+#define LED_5_B_SOURCE GPIO_PinSource9
+
+#define LED_6_R_GPIO_CLK RCC_APB2Periph_GPIOA
+#define LED_6_R_PIN GPIO_Pin_8
+#define LED_6_R_GPIO_PORT GPIOA
+#define LED_6_R_SOURCE GPIO_PinSource8
+
+#define LED_6_G_GPIO_CLK RCC_APB2Periph_GPIOA
+#define LED_6_G_PIN GPIO_Pin_11
+#define LED_6_G_GPIO_PORT GPIOA
+#define LED_6_G_SOURCE GPIO_PinSource11
+
+#define LED_6_B_GPIO_CLK RCC_APB2Periph_GPIOA
+#define LED_6_B_PIN GPIO_Pin_12
+#define LED_6_B_GPIO_PORT GPIOA
+#define LED_6_B_SOURCE GPIO_PinSource12
+
+#define START_PB_GPIO_CLK RCC_APB2Periph_GPIOA
+#define START_PB_PIN GPIO_Pin_15
+#define START_PB_GPIO_PORT GPIOA
+#define START_PB_SOURCE GPIO_PinSource15
+#define START_PB_EXTI_PORT EXTI_PortSourceGPIOA
+#define START_PB_EXTI_PIN EXTI_PinSource15
+#define START_PB_EXTI_LINE EXTI_Line15
+
+#define MODE_PB_GPIO_CLK RCC_APB2Periph_GPIOA
+#define MODE_PB_PIN GPIO_Pin_14
+#define MODE_PB_GPIO_PORT GPIOA
+#define MODE_PB_SOURCE GPIO_PinSource14
+#define MODE_PB_EXTI_PORT EXTI_PortSourceGPIOA
+#define MODE_PB_EXTI_PIN EXTI_PinSource14
+#define MODE_PB_EXTI_LINE EXTI_Line14
+
+#define GPO_TIMER1 TIM2
+#define GPO_TIMER1_CLK RCC_APB1Periph_TIM2
+#define GPO_TIMER1_IRQn TIM2_IRQn
+#define GPO_TIMER1_IRQHandler TIM2_IRQHandler
+
+#define GPO_TIMER2 TIM3
+#define GPO_TIMER2_CLK RCC_APB1Periph_TIM3
+#define GPO_TIMER2_IRQn TIM3_IRQn
+#define GPO_TIMER2_IRQHandler TIM3_IRQHandler
+
+#define GPO_TIMER3 TIM4
+#define GPO_TIMER3_CLK RCC_APB1Periph_TIM4
+#define GPO_TIMER3_IRQn TIM4_IRQn
+#define GPO_TIMER3_IRQHandler TIM4_IRQHandler
+
+#define GPI_EXTI1_IRQn EXTI15_10_IRQn
+#define GPI_EXTI1_IRQHandler EXTI15_10_IRQHandler
// private variables
-volatile uint16_t led2_blink;
-volatile uint16_t led3_blink;
-volatile uint16_t led4_blink;
-volatile uint16_t led_rx_blink;
-volatile uint16_t led_tx_blink;
-volatile uint8_t led_eye_r;
-volatile uint8_t led_eye_g;
-volatile uint8_t led_eye_b;
-volatile uint8_t led_head_r;
-volatile uint8_t led_head_g;
-volatile uint8_t led_head_b;
-// push button function pointers
-void (*mode_function)(void);
-void (*start_function)(void);
-
-// interrupt handler
-void TIM2_IRQHandler(void)
+// LED blink periods
+__IO uint16_t led_tx_period;
+__IO uint16_t led_rx_period;
+__IO uint16_t led_2_period;
+__IO uint16_t led_3_period;
+__IO uint16_t led_4_period;
+__IO uint16_t led_5_R_period;
+__IO uint16_t led_5_G_period;
+__IO uint16_t led_5_B_period;
+__IO uint16_t led_6_R_period;
+__IO uint16_t led_6_G_period;
+__IO uint16_t led_6_B_period;
+// Pushbuttons callbacks
+void (*start_pb_callback)(void);
+void (*mode_pb_callback)(void);
+
+// IRQ handler functions
+void GPI_EXTI1_IRQHandler(void)
+{
+ if(EXTI_GetITStatus(START_PB_EXTI_LINE) != RESET)
+ {
+ if(start_pb_callback!=0)
+ start_pb_callback();
+ /* Clear the EXTI line 0 pending bit */
+ EXTI_ClearITPendingBit(START_PB_EXTI_LINE);
+ }
+ if(EXTI_GetITStatus(MODE_PB_EXTI_LINE) != RESET)
+ {
+ if(mode_pb_callback!=0)
+ mode_pb_callback();
+ /* Clear the EXTI line 0 pending bit */
+ EXTI_ClearITPendingBit(MODE_PB_EXTI_LINE);
+ }
+}
+
+void GPO_TIMER1_IRQHandler(void)
{
uint16_t capture;
- static uint8_t duty_cycle=0x00;// counter for the RGB leds
- static uint16_t led2_blink_count=0x0000;
- static uint16_t led3_blink_count=0x0000;
- static uint16_t led4_blink_count=0x0000;
- static uint16_t led_rx_blink_count=0x0000;
- static uint16_t led_tx_blink_count=0x0000;
-
- if (TIM_GetITStatus(TIM2, TIM_IT_CC1) != RESET) // 10 KHz
+
+ if(TIM_GetITStatus(GPO_TIMER1, TIM_IT_CC1)!=RESET)
{
- TIM_ClearITPendingBit(TIM2, TIM_IT_CC1);
- // generate the PWM for the LED's color
- if(duty_cycle>=led_eye_r)
- GPIO_SetBits(PORT_EYE_LEDS,PIN_EYE_R_LED);
- else
- GPIO_ResetBits(PORT_EYE_LEDS,PIN_EYE_R_LED);
- if(duty_cycle>=led_eye_g)
- GPIO_SetBits(PORT_EYE_LEDS,PIN_EYE_G_LED);
+ TIM_ClearITPendingBit(GPO_TIMER1,TIM_IT_CC1);
+ if(GPIO_ReadOutputDataBit(LED_TX_GPIO_PORT,LED_TX_PIN))
+ GPIO_ResetBits(LED_TX_GPIO_PORT,LED_TX_PIN);
else
- GPIO_ResetBits(PORT_EYE_LEDS,PIN_EYE_G_LED);
- if(duty_cycle>=led_eye_b)
- GPIO_SetBits(PORT_EYE_LEDS,PIN_EYE_B_LED);
- else
- GPIO_ResetBits(PORT_EYE_LEDS,PIN_EYE_B_LED);
- if(duty_cycle>=led_head_r)
- GPIO_SetBits(PORT_HEAD_LEDS,PIN_HEAD_R_LED);
+ GPIO_SetBits(LED_TX_GPIO_PORT,LED_TX_PIN);
+ capture = TIM_GetCapture1(GPO_TIMER1);
+ TIM_SetCompare1(GPO_TIMER1, capture + led_tx_period);
+ }
+ if(TIM_GetITStatus(GPO_TIMER1, TIM_IT_CC2)!=RESET)
+ {
+ TIM_ClearITPendingBit(GPO_TIMER1,TIM_IT_CC2);
+ if(GPIO_ReadOutputDataBit(LED_RX_GPIO_PORT,LED_RX_PIN))
+ GPIO_ResetBits(LED_RX_GPIO_PORT,LED_RX_PIN);
else
- GPIO_ResetBits(PORT_HEAD_LEDS,PIN_HEAD_R_LED);
- if(duty_cycle>=led_head_g)
- GPIO_SetBits(PORT_HEAD_LEDS,PIN_HEAD_G_LED);
+ GPIO_SetBits(LED_RX_GPIO_PORT,LED_RX_PIN);
+ capture = TIM_GetCapture2(GPO_TIMER1);
+ TIM_SetCompare2(GPO_TIMER1, capture + led_rx_period);
+ }
+ if(TIM_GetITStatus(GPO_TIMER1, TIM_IT_CC3)!=RESET)
+ {
+ TIM_ClearITPendingBit(GPO_TIMER1,TIM_IT_CC3);
+ if(GPIO_ReadOutputDataBit(LED_2_GPIO_PORT,LED_2_PIN))
+ GPIO_ResetBits(LED_2_GPIO_PORT,LED_2_PIN);
else
- GPIO_ResetBits(PORT_HEAD_LEDS,PIN_HEAD_G_LED);
- if(duty_cycle>=led_head_b)
- GPIO_SetBits(PORT_HEAD_LEDS,PIN_HEAD_B_LED);
+ GPIO_SetBits(LED_2_GPIO_PORT,LED_2_PIN);
+ capture = TIM_GetCapture3(GPO_TIMER1);
+ TIM_SetCompare3(GPO_TIMER1, capture + led_2_period);
+ }
+ if(TIM_GetITStatus(GPO_TIMER1, TIM_IT_CC4)!=RESET)
+ {
+ TIM_ClearITPendingBit(GPO_TIMER1,TIM_IT_CC4);
+ if(GPIO_ReadOutputDataBit(LED_3_GPIO_PORT,LED_3_PIN))
+ GPIO_ResetBits(LED_3_GPIO_PORT,LED_3_PIN);
else
- GPIO_ResetBits(PORT_HEAD_LEDS,PIN_HEAD_B_LED);
- duty_cycle=(duty_cycle+1)&0x1F;
- capture = TIM_GetCapture1(TIM2);
- TIM_SetCompare1(TIM2, capture + color_gen_period);
+ GPIO_SetBits(LED_3_GPIO_PORT,LED_3_PIN);
+ capture = TIM_GetCapture4(GPO_TIMER1);
+ TIM_SetCompare4(GPO_TIMER1, capture + led_3_period);
}
- if (TIM_GetITStatus(TIM2, TIM_IT_CC2) != RESET) // 1kHz
+}
+
+void GPO_TIMER2_IRQHandler(void)
+{
+ uint16_t capture;
+
+ if(TIM_GetITStatus(GPO_TIMER2, TIM_IT_CC1)!=RESET)
{
- TIM_ClearITPendingBit(TIM2, TIM_IT_CC2);
- // periodically enable and disable the LED's
- if(led2_blink_count==0x0000)
- {
- if(led2_blink!=0x0000)
- {
- led2_blink_count=led2_blink;
- toggle_led2();
- }
- }
+ TIM_ClearITPendingBit(GPO_TIMER2,TIM_IT_CC1);
+ if(GPIO_ReadOutputDataBit(LED_5_R_GPIO_PORT,LED_5_R_PIN))
+ GPIO_ResetBits(LED_5_R_GPIO_PORT,LED_5_R_PIN);
else
- led2_blink_count--;
- if(led3_blink_count==0x0000)
- {
- if(led3_blink!=0x0000)
- {
- led3_blink_count=led3_blink;
- toggle_led3();
- }
- }
+ GPIO_SetBits(LED_5_R_GPIO_PORT,LED_5_R_PIN);
+ capture = TIM_GetCapture1(GPO_TIMER2);
+ TIM_SetCompare1(GPO_TIMER2, capture + led_5_R_period);
+ }
+ if(TIM_GetITStatus(GPO_TIMER2, TIM_IT_CC2)!=RESET)
+ {
+ TIM_ClearITPendingBit(GPO_TIMER2,TIM_IT_CC2);
+ if(GPIO_ReadOutputDataBit(LED_5_G_GPIO_PORT,LED_5_G_PIN))
+ GPIO_ResetBits(LED_5_G_GPIO_PORT,LED_5_G_PIN);
else
- led3_blink_count--;
- if(led4_blink_count==0x0000)
- {
- if(led4_blink!=0x0000)
- {
- led4_blink_count=led4_blink;
- toggle_led4();
- }
- }
+ GPIO_SetBits(LED_5_G_GPIO_PORT,LED_5_G_PIN);
+ capture = TIM_GetCapture2(GPO_TIMER2);
+ TIM_SetCompare2(GPO_TIMER2, capture + led_5_G_period);
+ }
+ if(TIM_GetITStatus(GPO_TIMER2, TIM_IT_CC3)!=RESET)
+ {
+ TIM_ClearITPendingBit(GPO_TIMER2,TIM_IT_CC3);
+ if(GPIO_ReadOutputDataBit(LED_5_B_GPIO_PORT,LED_5_B_PIN))
+ GPIO_ResetBits(LED_5_B_GPIO_PORT,LED_5_B_PIN);
else
- led4_blink_count--;
- if(led_rx_blink_count==0x0000)
- {
- if(led_rx_blink!=0x0000)
- {
- led_rx_blink_count=led_rx_blink;
- toggle_rx_led();
- }
- }
+ GPIO_SetBits(LED_5_B_GPIO_PORT,LED_5_B_PIN);
+ capture = TIM_GetCapture3(GPO_TIMER2);
+ TIM_SetCompare3(GPO_TIMER2, capture + led_5_B_period);
+ }
+ if(TIM_GetITStatus(GPO_TIMER2, TIM_IT_CC4)!=RESET)
+ {
+ TIM_ClearITPendingBit(GPO_TIMER2,TIM_IT_CC4);
+ if(GPIO_ReadOutputDataBit(LED_4_GPIO_PORT,LED_4_PIN))
+ GPIO_ResetBits(LED_4_GPIO_PORT,LED_4_PIN);
else
- led_rx_blink_count--;
- if(led_tx_blink_count==0x0000)
- {
- if(led_tx_blink!=0x0000)
- {
- led_tx_blink_count=led_tx_blink;
- toggle_tx_led();
- }
- }
+ GPIO_SetBits(LED_4_GPIO_PORT,LED_4_PIN);
+ capture = TIM_GetCapture4(GPO_TIMER2);
+ TIM_SetCompare4(GPO_TIMER2, capture + led_4_period);
+ }
+}
+
+void GPO_TIMER3_IRQHandler(void)
+{
+ uint16_t capture;
+
+ if(TIM_GetITStatus(GPO_TIMER3, TIM_IT_CC1)!=RESET)
+ {
+ TIM_ClearITPendingBit(GPO_TIMER3,TIM_IT_CC1);
+ if(GPIO_ReadOutputDataBit(LED_6_R_GPIO_PORT,LED_6_R_PIN))
+ GPIO_ResetBits(LED_6_R_GPIO_PORT,LED_6_R_PIN);
else
- led_tx_blink_count--;
- capture = TIM_GetCapture2(TIM2);
- TIM_SetCompare2(TIM2, capture + blink_period);
+ GPIO_SetBits(LED_6_R_GPIO_PORT,LED_6_R_PIN);
+ capture = TIM_GetCapture1(GPO_TIMER3);
+ TIM_SetCompare1(GPO_TIMER3, capture + led_6_R_period);
}
- if (TIM_GetITStatus(TIM2, TIM_IT_CC3) != RESET) // 10 Hz
+ if(TIM_GetITStatus(GPO_TIMER3, TIM_IT_CC2)!=RESET)
{
- TIM_ClearITPendingBit(TIM2, TIM_IT_CC3);
- // check the pushbuttons
- if(is_mode_pushed())
- {
- if(mode_function!=0)
- mode_function();
- ram_set_bit(CM730_BUTTON,0x00);
- }
+ TIM_ClearITPendingBit(GPO_TIMER3,TIM_IT_CC2);
+ if(GPIO_ReadOutputDataBit(LED_6_G_GPIO_PORT,LED_6_G_PIN))
+ GPIO_ResetBits(LED_6_G_GPIO_PORT,LED_6_G_PIN);
else
- ram_clear_bit(CM730_BUTTON,0x00);
- if(is_start_pushed())
- {
- if(start_function!=0)
- start_function();
- ram_set_bit(CM730_BUTTON,0x01);
- }
+ GPIO_SetBits(LED_6_G_GPIO_PORT,LED_6_G_PIN);
+ capture = TIM_GetCapture2(GPO_TIMER3);
+ TIM_SetCompare2(GPO_TIMER3, capture + led_6_G_period);
+ }
+ if(TIM_GetITStatus(GPO_TIMER3, TIM_IT_CC3)!=RESET)
+ {
+ TIM_ClearITPendingBit(GPO_TIMER3,TIM_IT_CC3);
+ if(GPIO_ReadOutputDataBit(LED_6_B_GPIO_PORT,LED_6_B_PIN))
+ GPIO_ResetBits(LED_6_B_GPIO_PORT,LED_6_B_PIN);
else
- ram_clear_bit(CM730_BUTTON,0x01);
- capture = TIM_GetCapture3(TIM2);
- TIM_SetCompare3(TIM2, capture + pushbuttons_period);
+ GPIO_SetBits(LED_6_B_GPIO_PORT,LED_6_B_PIN);
+ capture = TIM_GetCapture3(GPO_TIMER3);
+ TIM_SetCompare3(GPO_TIMER3, capture + led_6_B_period);
}
}
+// private functions
+
// public functions
void gpio_init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
- GPIO_StructInit(&GPIO_InitStructure);
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
- TIM_OCInitTypeDef TIM_OCInitStructure;
+ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
NVIC_InitTypeDef NVIC_InitStructure;
-
- // initialize the private variables
- led2_blink=0x0000;
- led3_blink=0x0000;
- led4_blink=0x0000;
- led_rx_blink=0x0000;
- led_tx_blink=0x0000;
- led_eye_r=0x00;
- led_eye_g=0x00;
- led_eye_b=0x00;
- led_head_r=0x00;
- led_head_g=0x00;
- led_head_b=0x00;
- mode_function=0;
- start_function=0;
-
- // configure the LED's
- GPIO_InitStructure.GPIO_Pin = PIN_LED2 | PIN_LED4;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ EXTI_InitTypeDef EXTI_InitStructure;
+
+ /* enable clocks */
+ RCC_APB2PeriphClockCmd(LED_RX_GPIO_CLK | LED_TX_GPIO_CLK | LED_2_GPIO_CLK | LED_3_GPIO_CLK | LED_4_GPIO_CLK |
+ LED_5_R_GPIO_CLK | LED_5_G_GPIO_CLK | LED_5_B_GPIO_CLK | LED_6_R_GPIO_CLK | LED_6_G_GPIO_CLK |
+ LED_6_B_GPIO_CLK, ENABLE);
+ RCC_APB2PeriphClockCmd(START_PB_GPIO_CLK | MODE_PB_GPIO_CLK, ENABLE);
+ RCC_APB1PeriphClockCmd(GPO_TIMER1_CLK | GPO_TIMER2_CLK | GPO_TIMER3_CLK,ENABLE);
+ RCC_APB1PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
+
+ /* GPIO Configuration */
+ GPIO_InitStructure.GPIO_Pin = LED_RX_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_Init(PORT_LED2, &GPIO_InitStructure);
-
- GPIO_InitStructure.GPIO_Pin = PIN_LED3;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_Init(PORT_LED3, &GPIO_InitStructure);
+ GPIO_Init(LED_RX_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = LED_TX_PIN;
+ GPIO_Init(LED_TX_GPIO_PORT, &GPIO_InitStructure);
- // configure the uart LED's
- GPIO_InitStructure.GPIO_Pin = PIN_RX_LED;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_Init(PORT_RX_LED, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = LED_2_PIN;
+ GPIO_Init(LED_2_GPIO_PORT, &GPIO_InitStructure);
- GPIO_InitStructure.GPIO_Pin = PIN_TX_LED;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_Init(PORT_TX_LED, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = LED_3_PIN;
+ GPIO_Init(LED_3_GPIO_PORT, &GPIO_InitStructure);
- // configure the RGB LED's
- GPIO_InitStructure.GPIO_Pin = PIN_EYE_R_LED | PIN_EYE_G_LED | PIN_EYE_B_LED;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_Init(PORT_EYE_LEDS, &GPIO_InitStructure);
-
- GPIO_InitStructure.GPIO_Pin = PIN_HEAD_R_LED | PIN_HEAD_G_LED | PIN_HEAD_B_LED;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_Init(PORT_HEAD_LEDS, &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = LED_4_PIN;
+ GPIO_Init(LED_4_GPIO_PORT, &GPIO_InitStructure);
- // configure the pushbuttons
- GPIO_InitStructure.GPIO_Pin = PIN_SW_MODE | PIN_SW_START ;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
- GPIO_Init(PORT_PUSHBUTTONS , &GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = LED_5_R_PIN;
+ GPIO_Init(LED_5_R_GPIO_PORT, &GPIO_InitStructure);
- // initialize the timer
- TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
- TIM_OCStructInit(&TIM_OCInitStructure);
+ GPIO_InitStructure.GPIO_Pin = LED_5_G_PIN;
+ GPIO_Init(LED_5_G_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = LED_5_B_PIN;
+ GPIO_Init(LED_5_B_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = LED_6_R_PIN;
+ GPIO_Init(LED_6_G_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = LED_6_G_PIN;
+ GPIO_Init(LED_6_G_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = LED_6_B_PIN;
+ GPIO_Init(LED_6_B_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = START_PB_PIN;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(START_PB_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = MODE_PB_PIN;
+ GPIO_Init(MODE_PB_GPIO_PORT, &GPIO_InitStructure);
+
+ // initialize private variables
+ led_rx_period=0;
+ led_rx_period=0;
+ led_2_period=0;
+ led_3_period=0;
+ led_4_period=0;
+ led_5_R_period=0;
+ led_5_G_period=0;
+ led_5_B_period=0;
+ led_6_R_period=0;
+ led_6_G_period=0;
+ led_6_B_period=0;
+ start_pb_callback=0;
+ mode_pb_callback=0;
// initialize the timer interrupts
- NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannel = GPO_TIMER1_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-
NVIC_Init(&NVIC_InitStructure);
- TIM_DeInit(TIM2);
- /* Time base configuration */
- TIM_TimeBaseStructure.TIM_Period = 65535;
+ /* LED's timer configuration */
+ TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
TIM_TimeBaseStructure.TIM_Prescaler = 0;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
- /* Prescaler configuration */
- TIM_PrescalerConfig(TIM2, 720, TIM_PSCReloadMode_Immediate);// set the timer time base to 100KHz
- TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
- TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
- TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
- /* Output Compare Timing Mode configuration: color generation */
- TIM_OCInitStructure.TIM_Pulse = color_gen_period;
- TIM_OC1Init(TIM2, &TIM_OCInitStructure);
- TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Disable);
- /* Output Compare Timing Mode configuration: blink generation */
- TIM_OCInitStructure.TIM_Pulse = blink_period;
- TIM_OC2Init(TIM2, &TIM_OCInitStructure);
- TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Disable);
- /* Output Compare Timing Mode configuration: pushbutton sampling */
- TIM_OCInitStructure.TIM_Pulse = pushbuttons_period;
- TIM_OC3Init(TIM2, &TIM_OCInitStructure);
- TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Disable);
- /* TIM IT enable */
- TIM_ITConfig(TIM2, TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3, ENABLE);
- /* TIM2 enable counter */
- TIM_Cmd(TIM2, ENABLE);
-
- // turn off all LED's
- clear_led2();
- clear_led3();
- clear_led4();
- clear_rx_led();
- clear_rx_led();
- set_eyes_color(0x00,0x08,0x00);
- set_head_color(0x00,0x08,0x00);
-}
+ TIM_TimeBaseInit(GPO_TIMER1,&TIM_TimeBaseStructure);
+ TIM_Cmd(GPO_TIMER1, ENABLE);
+ TIM_PrescalerConfig(GPO_TIMER1, 36000, TIM_PSCReloadMode_Immediate);
+ TIM_SetClockDivision(GPO_TIMER1,TIM_CKD_DIV2);
-// LED 2 functions
-void set_led2(void)
-{
- GPIO_ResetBits(PORT_LED2,PIN_LED2);
- ram_set_bit(CM730_LED_PANNEL,0x00);
-}
-
-void clear_led2(void)
-{
- GPIO_SetBits(PORT_LED2,PIN_LED2);
- ram_clear_bit(CM730_LED_PANNEL,0x00);
-}
+ // initialize the timer interrupts
+ NVIC_InitStructure.NVIC_IRQChannel = GPO_TIMER2_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
-void toggle_led2(void)
-{
- if(GPIO_ReadOutputDataBit(PORT_LED2,PIN_LED2))
- {
- GPIO_ResetBits(PORT_LED2,PIN_LED2);
- ram_clear_bit(CM730_LED_PANNEL,0x00);
- }
- else
- {
- GPIO_SetBits(PORT_LED2,PIN_LED2);
- ram_set_bit(CM730_LED_PANNEL,0x00);
- }
-}
+ /* LED's timer configuration */
+ TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
+ TIM_TimeBaseStructure.TIM_Prescaler = 0;
+ TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+ TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ TIM_TimeBaseInit(GPO_TIMER2,&TIM_TimeBaseStructure);
+ TIM_Cmd(GPO_TIMER2, ENABLE);
+ TIM_PrescalerConfig(GPO_TIMER2, 36000, TIM_PSCReloadMode_Immediate);
+ TIM_SetClockDivision(GPO_TIMER2,TIM_CKD_DIV2);
-void blink_led2(uint16_t period_ms)
-{
- // set a period of 0 to stop blinking
- led2_blink=(period_ms/2);
- if(period_ms==0x0000)
- clear_led2();
-}
+ // initialize the timer interrupts
+ NVIC_InitStructure.NVIC_IRQChannel = GPO_TIMER3_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
-// LED 3 functions
-void set_led3(void)
-{
- GPIO_ResetBits(PORT_LED3,PIN_LED3);
- ram_set_bit(CM730_LED_PANNEL,0x01);
-}
+ /* LED's timer configuration */
+ TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
+ TIM_TimeBaseStructure.TIM_Prescaler = 0;
+ TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+ TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ TIM_TimeBaseInit(GPO_TIMER3,&TIM_TimeBaseStructure);
+ TIM_Cmd(GPO_TIMER3, ENABLE);
+ TIM_PrescalerConfig(GPO_TIMER3, 36000, TIM_PSCReloadMode_Immediate);
+ TIM_SetClockDivision(GPO_TIMER3,TIM_CKD_DIV2);
+
+ /* configure external interrupts */
+ EXTI_InitStructure.EXTI_Line = START_PB_EXTI_LINE | MODE_PB_EXTI_LINE;
+ EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
+ EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
+ EXTI_InitStructure.EXTI_LineCmd = ENABLE;
+ EXTI_Init(&EXTI_InitStructure);
+
+ NVIC_InitStructure.NVIC_IRQChannel = GPI_EXTI1_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
-void clear_led3(void)
-{
- GPIO_SetBits(PORT_LED3,PIN_LED3);
- ram_clear_bit(CM730_LED_PANNEL,0x01);
+ // turn off all leds
+ gpio_clear_led(LED_TX);
+ gpio_clear_led(LED_RX);
+ gpio_clear_led(LED_2);
+ gpio_clear_led(LED_3);
+ gpio_clear_led(LED_4);
+ gpio_clear_led(LED_5_R);
+ gpio_clear_led(LED_5_G);
+ gpio_clear_led(LED_5_B);
+ gpio_clear_led(LED_6_R);
+ gpio_clear_led(LED_6_G);
+ gpio_clear_led(LED_6_B);
}
-void toggle_led3(void)
+void gpio_set_led(led_t led_id)
{
- if(GPIO_ReadOutputDataBit(PORT_LED3,PIN_LED3))
+ switch(led_id)
{
- GPIO_ResetBits(PORT_LED3,PIN_LED3);
- ram_clear_bit(CM730_LED_PANNEL,0x01);
- }
- else
- {
- GPIO_SetBits(PORT_LED3,PIN_LED3);
- ram_set_bit(CM730_LED_PANNEL,0x01);
+ case LED_TX:
+ GPIO_ResetBits(LED_TX_GPIO_PORT,LED_TX_PIN);
+ break;
+ case LED_RX:
+ GPIO_ResetBits(LED_RX_GPIO_PORT,LED_RX_PIN);
+ break;
+ case LED_2:
+ GPIO_ResetBits(LED_2_GPIO_PORT,LED_2_PIN);
+ break;
+ case LED_3:
+ GPIO_ResetBits(LED_3_GPIO_PORT,LED_3_PIN);
+ break;
+ case LED_4:
+ GPIO_ResetBits(LED_4_GPIO_PORT,LED_4_PIN);
+ break;
+ case LED_5_R:
+ GPIO_ResetBits(LED_5_R_GPIO_PORT,LED_5_R_PIN);
+ break;
+ case LED_5_G:
+ GPIO_ResetBits(LED_5_G_GPIO_PORT,LED_5_G_PIN);
+ break;
+ case LED_5_B:
+ GPIO_ResetBits(LED_5_B_GPIO_PORT,LED_5_B_PIN);
+ break;
+ case LED_6_R:
+ GPIO_ResetBits(LED_6_R_GPIO_PORT,LED_6_R_PIN);
+ break;
+ case LED_6_G:
+ GPIO_ResetBits(LED_6_G_GPIO_PORT,LED_6_G_PIN);
+ break;
+ case LED_6_B:
+ GPIO_ResetBits(LED_6_B_GPIO_PORT,LED_6_B_PIN);
+ break;
}
}
-void blink_led3(uint16_t period_ms)
+void gpio_clear_led(led_t led_id)
{
- // set a period of 0 to stop blinking
- led3_blink=(period_ms/2);
- if(period_ms==0x0000)
- clear_led3();
-}
-
-// LED 4 functions
-void set_led4(void)
-{
- GPIO_ResetBits(PORT_LED4,PIN_LED4);
- ram_set_bit(CM730_LED_PANNEL,0x02);
-}
-
-void clear_led4(void)
-{
- GPIO_SetBits(PORT_LED4,PIN_LED4);
- ram_clear_bit(CM730_LED_PANNEL,0x02);
-}
-
-void toggle_led4(void)
-{
- if(GPIO_ReadOutputDataBit(PORT_LED4,PIN_LED4))
- {
- GPIO_ResetBits(PORT_LED4,PIN_LED4);
- ram_clear_bit(CM730_LED_PANNEL,0x02);
- }
- else
+ switch(led_id)
{
- GPIO_SetBits(PORT_LED4,PIN_LED4);
- ram_set_bit(CM730_LED_PANNEL,0x02);
+ case LED_TX:
+ GPIO_SetBits(LED_TX_GPIO_PORT,LED_TX_PIN);
+ break;
+ case LED_RX:
+ GPIO_SetBits(LED_RX_GPIO_PORT,LED_RX_PIN);
+ break;
+ case LED_2:
+ GPIO_SetBits(LED_2_GPIO_PORT,LED_2_PIN);
+ break;
+ case LED_3:
+ GPIO_SetBits(LED_3_GPIO_PORT,LED_3_PIN);
+ break;
+ case LED_4:
+ GPIO_SetBits(LED_4_GPIO_PORT,LED_4_PIN);
+ break;
+ case LED_5_R:
+ GPIO_SetBits(LED_5_R_GPIO_PORT,LED_5_R_PIN);
+ break;
+ case LED_5_G:
+ GPIO_SetBits(LED_5_G_GPIO_PORT,LED_5_G_PIN);
+ break;
+ case LED_5_B:
+ GPIO_SetBits(LED_5_B_GPIO_PORT,LED_5_B_PIN);
+ break;
+ case LED_6_R:
+ GPIO_SetBits(LED_6_R_GPIO_PORT,LED_6_R_PIN);
+ break;
+ case LED_6_G:
+ GPIO_SetBits(LED_6_G_GPIO_PORT,LED_6_G_PIN);
+ break;
+ case LED_6_B:
+ GPIO_SetBits(LED_6_B_GPIO_PORT,LED_6_B_PIN);
+ break;
}
}
-void blink_led4(uint16_t period_ms)
+void gpio_toggle_led(led_t led_id)
{
- // set a period of 0 to stop blinking
- led4_blink=(period_ms/2);
- if(period_ms==0x0000)
- clear_led4();
-}
-
-// RX LED functions
-void set_rx_led(void)
-{
- GPIO_ResetBits(PORT_RX_LED,PIN_RX_LED);
-}
-
-void clear_rx_led(void)
-{
- GPIO_SetBits(PORT_RX_LED,PIN_RX_LED);
-}
-
-void toggle_rx_led(void)
-{
- if(GPIO_ReadOutputDataBit(PORT_RX_LED,PIN_RX_LED))
- GPIO_ResetBits(PORT_RX_LED,PIN_RX_LED);
- else
- GPIO_SetBits(PORT_RX_LED,PIN_RX_LED);
-}
-
-void blink_rx_led(uint16_t period_ms)
-{
- // set a period of 0 to stop blinking
- led_rx_blink=(period_ms/2);
- if(period_ms==0x0000)
- clear_rx_led();
-}
-
-// TX LED functions
-void set_tx_led(void)
-{
- GPIO_ResetBits(PORT_TX_LED,PIN_TX_LED);
-}
-
-void clear_tx_led(void)
-{
- GPIO_SetBits(PORT_TX_LED,PIN_TX_LED);
-}
-
-void toggle_tx_led(void)
-{
- if(GPIO_ReadOutputDataBit(PORT_TX_LED,PIN_TX_LED))
- GPIO_ResetBits(PORT_TX_LED,PIN_TX_LED);
- else
- GPIO_SetBits(PORT_TX_LED,PIN_TX_LED);
-}
-
-void blink_tx_led(uint16_t period_ms)
-{
- // set a period of 0 to stop blinking
- led_tx_blink=(period_ms/2);
- if(period_ms==0x0000)
- clear_tx_led();
-}
-
-// RGB LED's
-void set_eyes_color(uint8_t r,uint8_t g, uint8_t b)
-{
- uint16_t color;
-
- led_eye_r=(r&0x1F);
- led_eye_g=(g&0x1F);
- led_eye_b=(b&0x1F);
- color=led_eye_r+(led_eye_g<<5)+(led_eye_b<<10);
- ram_write_word(CM730_LED_HEAD_L,color);
-}
-
-void get_eyes_color(uint8_t *r,uint8_t *g, uint8_t *b)
-{
- (*r)=(led_eye_r&0x1F);
- (*g)=(led_eye_g&0x1F);
- (*b)=(led_eye_b&0x1F);
-}
-
-void set_head_color(uint8_t r,uint8_t g, uint8_t b)
-{
- uint16_t color;
-
- led_head_r=(r&0x1F);
- led_head_g=(g&0x1F);
- led_head_b=(b&0x1F);
- color=led_head_r+(led_head_g<<5)+(led_head_b<<10);
- ram_write_word(CM730_LED_EYE_L,color);
+ switch(led_id)
+ {
+ case LED_TX:
+ if(GPIO_ReadOutputDataBit(LED_TX_GPIO_PORT,LED_TX_PIN))
+ GPIO_ResetBits(LED_TX_GPIO_PORT,LED_TX_PIN);
+ else
+ GPIO_SetBits(LED_TX_GPIO_PORT,LED_TX_PIN);
+ break;
+ case LED_RX:
+ if(GPIO_ReadOutputDataBit(LED_RX_GPIO_PORT,LED_RX_PIN))
+ GPIO_ResetBits(LED_RX_GPIO_PORT,LED_RX_PIN);
+ else
+ GPIO_SetBits(LED_RX_GPIO_PORT,LED_RX_PIN);
+ break;
+ case LED_2:
+ if(GPIO_ReadOutputDataBit(LED_2_GPIO_PORT,LED_2_PIN))
+ GPIO_ResetBits(LED_2_GPIO_PORT,LED_2_PIN);
+ else
+ GPIO_SetBits(LED_2_GPIO_PORT,LED_2_PIN);
+ break;
+ case LED_3:
+ if(GPIO_ReadOutputDataBit(LED_3_GPIO_PORT,LED_3_PIN))
+ GPIO_ResetBits(LED_3_GPIO_PORT,LED_3_PIN);
+ else
+ GPIO_SetBits(LED_3_GPIO_PORT,LED_3_PIN);
+ break;
+ case LED_4:
+ if(GPIO_ReadOutputDataBit(LED_4_GPIO_PORT,LED_4_PIN))
+ GPIO_ResetBits(LED_4_GPIO_PORT,LED_4_PIN);
+ else
+ GPIO_SetBits(LED_4_GPIO_PORT,LED_4_PIN);
+ break;
+ case LED_5_R:
+ if(GPIO_ReadOutputDataBit(LED_5_R_GPIO_PORT,LED_5_R_PIN))
+ GPIO_ResetBits(LED_5_R_GPIO_PORT,LED_5_R_PIN);
+ else
+ GPIO_SetBits(LED_5_R_GPIO_PORT,LED_5_R_PIN);
+ break;
+ case LED_5_G:
+ if(GPIO_ReadOutputDataBit(LED_5_G_GPIO_PORT,LED_5_G_PIN))
+ GPIO_ResetBits(LED_5_G_GPIO_PORT,LED_5_G_PIN);
+ else
+ GPIO_SetBits(LED_5_G_GPIO_PORT,LED_5_G_PIN);
+ break;
+ case LED_5_B:
+ if(GPIO_ReadOutputDataBit(LED_5_B_GPIO_PORT,LED_5_B_PIN))
+ GPIO_ResetBits(LED_5_B_GPIO_PORT,LED_5_B_PIN);
+ else
+ GPIO_SetBits(LED_5_B_GPIO_PORT,LED_5_B_PIN);
+ break;
+ case LED_6_R:
+ if(GPIO_ReadOutputDataBit(LED_6_R_GPIO_PORT,LED_6_R_PIN))
+ GPIO_ResetBits(LED_6_R_GPIO_PORT,LED_6_R_PIN);
+ else
+ GPIO_SetBits(LED_6_R_GPIO_PORT,LED_6_R_PIN);
+ break;
+ case LED_6_G:
+ if(GPIO_ReadOutputDataBit(LED_6_G_GPIO_PORT,LED_6_G_PIN))
+ GPIO_ResetBits(LED_6_G_GPIO_PORT,LED_6_G_PIN);
+ else
+ GPIO_SetBits(LED_6_G_GPIO_PORT,LED_6_G_PIN);
+ break;
+ case LED_6_B:
+ if(GPIO_ReadOutputDataBit(LED_6_B_GPIO_PORT,LED_6_B_PIN))
+ GPIO_ResetBits(LED_6_B_GPIO_PORT,LED_6_B_PIN);
+ else
+ GPIO_SetBits(LED_6_B_GPIO_PORT,LED_6_B_PIN);
+ break;
+ }
}
-void get_head_color(uint8_t *r,uint8_t *g, uint8_t *b)
+void gpio_blink_led(led_t led_id, int16_t period_ms)
{
- (*r)=(led_head_r&0x1F);
- (*g)=(led_head_g&0x1F);
- (*b)=(led_head_b&0x1F);
-}
+ TIM_OCInitTypeDef TIM_OCInitStructure;
+ uint16_t capture;
-// pushbuttons functions
-uint8_t is_mode_pushed(void)
-{
- if(GPIO_ReadInputDataBit(PORT_PUSHBUTTONS,PIN_SW_MODE)==Bit_SET)
- return 0x00;
- else
- return 0x01;
+ TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
+ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
+ TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+ switch(led_id)
+ {
+ case LED_TX:
+ if(period_ms>1)
+ {
+ led_tx_period=period_ms;
+ TIM_OCInitStructure.TIM_Pulse = led_tx_period;
+ TIM_OC1Init(GPO_TIMER1, &TIM_OCInitStructure);
+ TIM_OC1PreloadConfig(GPO_TIMER1, TIM_OCPreload_Disable);
+ capture = TIM_GetCounter(GPO_TIMER1);
+ TIM_SetCompare1(GPO_TIMER1, capture + led_tx_period);
+ TIM_ITConfig(GPO_TIMER1, TIM_IT_CC1, ENABLE);
+ }
+ else
+ TIM_ITConfig(GPO_TIMER1, TIM_IT_CC1, DISABLE);
+ break;
+ case LED_RX:
+ if(period_ms>1)
+ {
+ led_rx_period=period_ms;
+ TIM_OCInitStructure.TIM_Pulse = led_rx_period;
+ TIM_OC2Init(GPO_TIMER1, &TIM_OCInitStructure);
+ TIM_OC2PreloadConfig(GPO_TIMER1, TIM_OCPreload_Disable);
+ capture = TIM_GetCounter(GPO_TIMER1);
+ TIM_SetCompare2(GPO_TIMER1, capture + led_rx_period);
+ TIM_ITConfig(GPO_TIMER1, TIM_IT_CC2, ENABLE);
+ }
+ else
+ TIM_ITConfig(GPO_TIMER1, TIM_IT_CC2, DISABLE);
+ break;
+ case LED_2:
+ if(period_ms>1)
+ {
+ led_2_period=period_ms;
+ TIM_OCInitStructure.TIM_Pulse = led_2_period;
+ TIM_OC3Init(GPO_TIMER1, &TIM_OCInitStructure);
+ TIM_OC3PreloadConfig(GPO_TIMER1, TIM_OCPreload_Disable);
+ capture = TIM_GetCounter(GPO_TIMER1);
+ TIM_SetCompare3(GPO_TIMER1, capture + led_2_period);
+ TIM_ITConfig(GPO_TIMER1, TIM_IT_CC3, ENABLE);
+ }
+ else
+ TIM_ITConfig(GPO_TIMER1, TIM_IT_CC3, DISABLE);
+ break;
+ case LED_3:
+ if(period_ms>1)
+ {
+ led_3_period=period_ms;
+ TIM_OCInitStructure.TIM_Pulse = led_3_period;
+ TIM_OC4Init(GPO_TIMER1, &TIM_OCInitStructure);
+ TIM_OC4PreloadConfig(GPO_TIMER1, TIM_OCPreload_Disable);
+ capture = TIM_GetCounter(GPO_TIMER1);
+ TIM_SetCompare4(GPO_TIMER1, capture + led_3_period);
+ TIM_ITConfig(GPO_TIMER1, TIM_IT_CC4, ENABLE);
+ }
+ else
+ TIM_ITConfig(GPO_TIMER1, TIM_IT_CC4, DISABLE);
+ break;
+ case LED_4:
+ if(period_ms>1)
+ {
+ led_4_period=period_ms;
+ TIM_OCInitStructure.TIM_Pulse = led_4_period;
+ TIM_OC4Init(GPO_TIMER2, &TIM_OCInitStructure);
+ TIM_OC4PreloadConfig(GPO_TIMER2, TIM_OCPreload_Disable);
+ capture = TIM_GetCounter(GPO_TIMER2);
+ TIM_SetCompare4(GPO_TIMER2, capture + led_4_period);
+ TIM_ITConfig(GPO_TIMER2, TIM_IT_CC4, ENABLE);
+ }
+ else
+ TIM_ITConfig(GPO_TIMER2, TIM_IT_CC4, DISABLE);
+ break;
+ case LED_5_R:
+ if(period_ms>1)
+ {
+ led_5_R_period=period_ms;
+ TIM_OCInitStructure.TIM_Pulse = led_5_R_period;
+ TIM_OC1Init(GPO_TIMER2, &TIM_OCInitStructure);
+ TIM_OC1PreloadConfig(GPO_TIMER2, TIM_OCPreload_Disable);
+ capture = TIM_GetCounter(GPO_TIMER2);
+ TIM_SetCompare1(GPO_TIMER2, capture + led_5_R_period);
+ TIM_ITConfig(GPO_TIMER2, TIM_IT_CC1, ENABLE);
+ }
+ else
+ TIM_ITConfig(GPO_TIMER2, TIM_IT_CC1, DISABLE);
+ break;
+ case LED_5_G:
+ if(period_ms>1)
+ {
+ led_5_G_period=period_ms;
+ TIM_OCInitStructure.TIM_Pulse = led_5_G_period;
+ TIM_OC2Init(GPO_TIMER2, &TIM_OCInitStructure);
+ TIM_OC2PreloadConfig(GPO_TIMER2, TIM_OCPreload_Disable);
+ capture = TIM_GetCounter(GPO_TIMER2);
+ TIM_SetCompare2(GPO_TIMER2, capture + led_5_G_period);
+ TIM_ITConfig(GPO_TIMER2, TIM_IT_CC2, ENABLE);
+ }
+ else
+ TIM_ITConfig(GPO_TIMER2, TIM_IT_CC2, DISABLE);
+ break;
+ case LED_5_B:
+ if(period_ms>1)
+ {
+ led_5_B_period=period_ms;
+ TIM_OCInitStructure.TIM_Pulse = led_5_B_period;
+ TIM_OC3Init(GPO_TIMER2, &TIM_OCInitStructure);
+ TIM_OC3PreloadConfig(GPO_TIMER2, TIM_OCPreload_Disable);
+ capture = TIM_GetCounter(GPO_TIMER2);
+ TIM_SetCompare3(GPO_TIMER2, capture + led_5_B_period);
+ TIM_ITConfig(GPO_TIMER2, TIM_IT_CC3, ENABLE);
+ }
+ else
+ TIM_ITConfig(GPO_TIMER2, TIM_IT_CC3, DISABLE);
+ break;
+ case LED_6_R:
+ if(period_ms>1)
+ {
+ led_6_R_period=period_ms;
+ TIM_OCInitStructure.TIM_Pulse = led_6_R_period;
+ TIM_OC1Init(GPO_TIMER3, &TIM_OCInitStructure);
+ TIM_OC1PreloadConfig(GPO_TIMER3, TIM_OCPreload_Disable);
+ capture = TIM_GetCounter(GPO_TIMER3);
+ TIM_SetCompare1(GPO_TIMER3, capture + led_6_R_period);
+ TIM_ITConfig(GPO_TIMER3, TIM_IT_CC1, ENABLE);
+ }
+ else
+ TIM_ITConfig(GPO_TIMER3, TIM_IT_CC1, DISABLE);
+ break;
+ case LED_6_G:
+ if(period_ms>1)
+ {
+ led_6_G_period=period_ms;
+ TIM_OCInitStructure.TIM_Pulse = led_6_G_period;
+ TIM_OC2Init(GPO_TIMER3, &TIM_OCInitStructure);
+ TIM_OC2PreloadConfig(GPO_TIMER3, TIM_OCPreload_Disable);
+ capture = TIM_GetCounter(GPO_TIMER3);
+ TIM_SetCompare2(GPO_TIMER3, capture + led_6_G_period);
+ TIM_ITConfig(GPO_TIMER3, TIM_IT_CC2, ENABLE);
+ }
+ else
+ TIM_ITConfig(GPO_TIMER3, TIM_IT_CC2, DISABLE);
+ break;
+ case LED_6_B:
+ if(period_ms>1)
+ {
+ led_6_B_period=period_ms;
+ TIM_OCInitStructure.TIM_Pulse = led_6_B_period;
+ TIM_OC3Init(GPO_TIMER3, &TIM_OCInitStructure);
+ TIM_OC3PreloadConfig(GPO_TIMER3, TIM_OCPreload_Disable);
+ capture = TIM_GetCounter(GPO_TIMER3);
+ TIM_SetCompare3(GPO_TIMER3, capture + led_6_B_period);
+ TIM_ITConfig(GPO_TIMER3, TIM_IT_CC3, ENABLE);
+ }
+ else
+ TIM_ITConfig(GPO_TIMER3, TIM_IT_CC3, DISABLE);
+ break;
+ default: break;
+ }
}
-void mode_attach_function(void (*sw_function)(void))
+uint8_t gpio_is_pushbutton_pressed(pushbutton_t pb_id)
{
- mode_function=sw_function;
-}
+ switch(pb_id)
+ {
+ case START_PB:
+ if(GPIO_ReadInputDataBit(START_PB_GPIO_PORT,START_PB_PIN)==Bit_SET)
+ return 0x01;
+ else
+ return 0x00;
+ break;
+ case MODE_PB:
+ if(GPIO_ReadInputDataBit(MODE_PB_GPIO_PORT,MODE_PB_PIN)==Bit_SET)
+ return 0x01;
+ else
+ return 0x00;
+ break;
+ }
-uint8_t is_start_pushed(void)
-{
- if(GPIO_ReadInputDataBit(PORT_PUSHBUTTONS,PIN_SW_START)==Bit_SET)
- return 0x00;
- else
- return 0x01;
+ return 0x00;
}
-void start_attach_function(void (*sw_function)(void))
+void gpio_set_pushbutton_callback(pushbutton_t pb_id,void (*callback)(void))
{
- start_function=sw_function;
+ switch(pb_id)
+ {
+ case START_PB:
+ start_pb_callback=callback;
+ break;
+ case MODE_PB:
+ mode_pb_callback=callback;
+ break;
+ }
}
diff --git a/src/imu.c b/src/imu.c
index 186dffc3a32ba5e5b1c82e47f23d3f98d778d3ce..c7c6a3960cc09242ea647a082d2abd463c614187 100755
--- a/src/imu.c
+++ b/src/imu.c
@@ -1,54 +1,45 @@
-#include "time.h"
-#include "gpio.h"
#include "imu.h"
-#include "spi.h"
#include "ram.h"
-#include <math.h>
+#include "gpio.h"
-#define PIN_SIG_ACC_CS GPIO_Pin_10
-#define PIN_SIG_GYRO_CS GPIO_Pin_11
-#define PORT_SPI_CS GPIOC
+#define IMU_SPI SPI2
+#define IMU_SPI_CLK RCC_APB1Periph_SPI2
+#define IMU_SPI_CLK_INIT RCC_APB1PeriphClockCmd
+#define IMU_SPI_IRQn SPI2_IRQn
+#define IMU_SPI_IRQHANDLER SPI2_IRQHandler
-// gyroscope registers
-#define GYRO_WHO_AM_I 0x0F
-#define GYRO_CTRL_REG1 0x20
-#define GYRO_CTRL_REG2 0x21
-#define GYRO_CTRL_REG3 0x22
-#define GYRO_CTRL_REG4 0x23
-#define GYRO_CTRL_REG5 0x24
-#define GYRO_REFERENCE 0x25
-#define GYRO_OUT_TEMP 0x26
-#define GYRO_STATUS_REG 0x27
-#define GYRO_OUT_X_L 0x28
-#define GYRO_OUT_X_H 0x29
-#define GYRO_OUT_Y_L 0x2A
-#define GYRO_OUT_Y_H 0x2B
-#define GYRO_OUT_Z_L 0x2C
-#define GYRO_OUT_Z_H 0x2D
-#define GYRO_FIFO_CTRL_REG 0x2E
-#define GYRO_FIFO_SRC_REG 0x2F
-#define GYRO_INT1_CFG 0x30
-#define GYRO_INT1_SRC 0x31
-#define GYRO_INT1_TSH_XH 0x32
-#define GYRO_INT1_TSH_XL 0x33
-#define GYRO_INT1_TSH_YH 0x34
-#define GYRO_INT1_TSH_YL 0x35
-#define GYRO_INT1_TSH_ZH 0x36
-#define GYRO_INT1_TSH_ZL 0x37
-#define GYRO_INT1_DURATION 0x38
-
-#define IMU_GYRO_CAL_NUM_SAMPLES 100
-#define IMU_GYRO_MAX_STD_DEV 5
+#define IMU_ACCEL_CS_PIN GPIO_Pin_10
+#define IMU_ACCEL_CS_PORT GPIOC
+#define IMU_ACCEL_CS_CLK RCC_APB2Periph_GPIOC
+#define IMU_GYRO_CS_PIN GPIO_Pin_11
+#define IMU_GYRO_CS_PORT GPIOC
+#define IMU_GYRO_CS_CLK RCC_APB2Periph_GPIOC
+
+#define IMU_SCK_PIN GPIO_Pin_13
+#define IMU_SCK_PORT GPIOB
+#define IMU_SCK_CLK RCC_APB2Periph_GPIOB
+#define IMU_MISO_PIN GPIO_Pin_14
+#define IMU_MISO_PORT GPIOB
+#define IMU_MISO_CLK RCC_APB2Periph_GPIOB
+#define IMU_MOSI_PIN GPIO_Pin_15
+#define IMU_MOSI_PORT GPIOB
+#define IMU_MOSI_CLK RCC_APB2Periph_GPIOB
+
+#define IMU_TIMER TIM6
+#define IMU_TIMER_IRQn TIM6_IRQn
+#define IMU_TIMER_IRQHandler TIM6_IRQHandler
+#define IMU_TIMER_CLK RCC_APB1Periph_TIM6
// accelerometer registers
+#define ACCEL_ID 0x32
#define ACCEL_WHO_AM_I 0x0F
#define ACCEL_CTRL_REG1 0x20
#define ACCEL_CTRL_REG2 0x21
#define ACCEL_CTRL_REG3 0x22
#define ACCEL_CTRL_REG4 0x23
#define ACCEL_CTRL_REG5 0x24
-#define ACCEL_REFERENCE 0x25
-#define ACCEL_OUT_TEMP 0x26
+#define ACCEL_HP_FILTER_RESET 0x25
+#define ACCEL_REFERENCE 0x26
#define ACCEL_STATUS_REG 0x27
#define ACCEL_OUT_X_L 0x28
#define ACCEL_OUT_X_H 0x29
@@ -66,239 +57,438 @@
#define ACCEL_INT1_TSH_ZL 0x37
#define ACCEL_INT1_DURATION 0x38
+// gyroscope registers
+#define GYRO_ID 0xD3
+#define GYRO_WHO_AM_I 0x0F
+#define GYRO_CTRL_REG1 0x20
+#define GYRO_CTRL_REG2 0x21
+#define GYRO_CTRL_REG3 0x22
+#define GYRO_CTRL_REG4 0x23
+#define GYRO_CTRL_REG5 0x24
+#define GYRO_REFERENCE 0x25
+#define GYRO_OUT_TEMP 0x26
+#define GYRO_STATUS_REG 0x27
+#define GYRO_OUT_X_L 0x28
+#define GYRO_OUT_X_H 0x29
+#define GYRO_OUT_Y_L 0x2A
+#define GYRO_OUT_Y_H 0x2B
+#define GYRO_OUT_Z_L 0x2C
+#define GYRO_OUT_Z_H 0x2D
+#define GYRO_FIFO_CTRL_REG 0x2E
+#define GYRO_FIFO_SRC_REG 0x2F
+#define GYRO_INT1_CFG 0x30
+#define GYRO_INT1_SRC 0x31
+#define GYRO_INT1_TSH_XH 0x32
+#define GYRO_INT1_TSH_XL 0x33
+#define GYRO_INT1_TSH_YH 0x34
+#define GYRO_INT1_TSH_YL 0x35
+#define GYRO_INT1_TSH_ZH 0x36
+#define GYRO_INT1_TSH_ZL 0x37
+#define GYRO_INT1_DURATION 0x38
+
+typedef enum {IMU_DONE=0,IMU_BUSY} imu_op_t;
+typedef enum {IMU_GET_ACCEL,IMU_GET_GYRO} imu_state_t;
+
// private variables
-const __IO uint16_t spi_period=400;
+uint8_t imu_spi_rd_data[32];
+uint8_t imu_spi_num_rd_data;
+uint8_t imu_spi_wr_data[32];
+uint8_t imu_spi_num_wr_data;
+volatile imu_op_t imu_op_state;
+uint8_t imu_current_device_id;
+uint8_t imu_stop_flag;
+volatile uint8_t imu_stopped;
// gyroscope variables
+const uint8_t gyro_conf_reg=GYRO_CTRL_REG1;
+const uint8_t gyro_conf_len=6;
+uint8_t gyro_conf_data[6];
+const uint8_t gyro_data_reg=GYRO_OUT_X_L;
+const uint8_t gyro_data_len=6;
int16_t gyro_x_center;
int16_t gyro_y_center;
int16_t gyro_z_center;
+// accelerometer variables
+const uint8_t accel_conf_reg=ACCEL_CTRL_REG1;
+const uint8_t accel_conf_len=5;
+uint8_t accel_conf_data[5];
+const uint8_t accel_data_reg=ACCEL_OUT_X_L;
+const uint8_t accel_data_len=6;
// private functions
-void imu_init_gyroscope(void)
+inline void imu_wait_op_done(void)
+{
+ while(imu_op_state!=IMU_DONE);
+}
+
+uint8_t imu_is_op_done(void)
+{
+ if(imu_op_state==IMU_DONE)
+ return 0x01;
+ else
+ return 0x00;
+}
+
+void imu_enable_device(uint8_t device_id)
{
- gyro_x_center=0x0000;
- gyro_y_center=0x0000;
- gyro_z_center=0x0000;
-
- GPIO_ResetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
- spi_write_byte(GYRO_CTRL_REG1,0xFF);
- GPIO_SetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
- GPIO_ResetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
- spi_write_byte(GYRO_CTRL_REG4,0x20);
- GPIO_SetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
+ if(device_id==ACCEL_ID)
+ GPIO_ResetBits(IMU_ACCEL_CS_PORT,IMU_ACCEL_CS_PIN);
+ else if(device_id==GYRO_ID)
+ GPIO_ResetBits(IMU_GYRO_CS_PORT,IMU_GYRO_CS_PIN);
+ imu_current_device_id=device_id;
}
-void imu_init_accelerometer(void)
+void imu_disable_device(void)
{
- GPIO_ResetBits(PORT_SPI_CS,PIN_SIG_ACC_CS);
- spi_write_byte(GYRO_CTRL_REG1,0x3F);
- GPIO_ResetBits(PORT_SPI_CS,PIN_SIG_ACC_CS);
- GPIO_SetBits(PORT_SPI_CS,PIN_SIG_ACC_CS);
- spi_write_byte(GYRO_CTRL_REG4,0x10);
- GPIO_SetBits(PORT_SPI_CS,PIN_SIG_ACC_CS);
+ if(imu_current_device_id==ACCEL_ID)
+ GPIO_SetBits(IMU_ACCEL_CS_PORT,IMU_ACCEL_CS_PIN);
+ else if(imu_current_device_id==GYRO_ID)
+ GPIO_SetBits(IMU_GYRO_CS_PORT,IMU_GYRO_CS_PIN);
+ imu_current_device_id=0x00;
}
-uint16_t imu_gyro_convert_data(int16_t data)
+void imu_spi_start_read(uint8_t address,uint8_t length)
{
- int16_t value;
+ uint8_t i;
- value = (data /64);
- value = (value * 5) / 4;
- value = 512 + value;
- if(value > 1023) value = 1023;
- if(value < 0) value = 0;
+ imu_spi_num_rd_data=length+1;
+ imu_spi_num_wr_data=length+1;
- return value;
+ imu_spi_wr_data[0]=address | 0xC0;
+ imu_spi_rd_data[0]=0xFF;
+ for(i=1;i<=length;i++)
+ {
+ imu_spi_wr_data[i]=0xFF;
+ imu_spi_rd_data[i]=0xFF;
+ }
+ imu_op_state=IMU_BUSY;
+ // start the read operation
+ SPI_I2S_ITConfig(IMU_SPI, SPI_I2S_IT_TXE, ENABLE);
}
-uint16_t imu_accel_convert_data(int16_t data)
+void imu_spi_get_data(uint8_t *data)
{
- int16_t value;
-
- value = (data /64);
- value = 512 + value;
- if(value > 1023) value = 1023;
- if(value < 0) value = 0;
-
- return value;
+ uint8_t i;
+
+ for(i=0;i<imu_spi_num_rd_data;i++)
+ data[i]=imu_spi_rd_data[i+1];
}
-// interrupt handlers
-void TIM4_IRQHandler(void)
+void imu_spi_start_write(uint8_t address,uint8_t *data,uint8_t length)
{
- uint16_t capture;
- int16_t sensor_value;
+ uint8_t i;
- if (TIM_GetITStatus(TIM4, TIM_IT_CC1) != RESET) // 8 ms
+ imu_spi_num_rd_data=length+1;
+ imu_spi_num_wr_data=length+1;
+
+ imu_spi_wr_data[0]=address | 0x40;
+ imu_spi_rd_data[0]=0xFF;
+ for(i=1;i<=length;i++)
{
- TIM_ClearITPendingBit(TIM4, TIM_IT_CC1);
- // read the gyroscope
- GPIO_ResetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
- spi_read_word(GYRO_OUT_X_L,(uint16_t *)&sensor_value);
- GPIO_SetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
- ram_write_word(CM730_GYRO_X_L,imu_gyro_convert_data(sensor_value));
- GPIO_ResetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
- spi_read_word(GYRO_OUT_Y_L,(uint16_t *)&sensor_value);
- GPIO_SetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
- ram_write_word(CM730_GYRO_Y_L,imu_gyro_convert_data(sensor_value));
- GPIO_ResetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
- spi_read_word(GYRO_OUT_Z_L,(uint16_t *)&sensor_value);
- GPIO_SetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
- ram_write_word(CM730_GYRO_Z_L,imu_gyro_convert_data(sensor_value));
- // read the accelerometer
- GPIO_ResetBits(PORT_SPI_CS,PIN_SIG_ACC_CS);
- spi_read_word(ACCEL_OUT_X_L,(uint16_t *)&sensor_value);
- GPIO_SetBits(PORT_SPI_CS,PIN_SIG_ACC_CS);
- ram_write_word(CM730_ACCEL_X_L,imu_accel_convert_data(sensor_value));
- GPIO_ResetBits(PORT_SPI_CS,PIN_SIG_ACC_CS);
- spi_read_word(ACCEL_OUT_Y_L,(uint16_t *)&sensor_value);
- GPIO_SetBits(PORT_SPI_CS,PIN_SIG_ACC_CS);
- ram_write_word(CM730_ACCEL_Y_L,imu_accel_convert_data(sensor_value));
- GPIO_ResetBits(PORT_SPI_CS,PIN_SIG_ACC_CS);
- spi_read_word(ACCEL_OUT_Z_L,(uint16_t *)&sensor_value);
- GPIO_SetBits(PORT_SPI_CS,PIN_SIG_ACC_CS);
- ram_write_word(CM730_ACCEL_Z_L,imu_accel_convert_data(sensor_value));
- capture = TIM_GetCapture1(TIM4);
- TIM_SetCompare1(TIM4, capture + spi_period);
+ imu_spi_wr_data[i]=data[i-1];
+ imu_spi_rd_data[i]=0xFF;
}
+ imu_op_state=IMU_BUSY;
+ // start the read operation
+ SPI_I2S_ITConfig(IMU_SPI, SPI_I2S_IT_TXE, ENABLE);
}
-void imu_calibrate_gyroscope(void)
+// accelerometer private functions
+uint8_t imu_accel_detect(void)
{
- int16_t x_samples[IMU_GYRO_CAL_NUM_SAMPLES];
- int16_t y_samples[IMU_GYRO_CAL_NUM_SAMPLES];
- int16_t z_samples[IMU_GYRO_CAL_NUM_SAMPLES];
- uint8_t i,calibrated=0x00;
- int32_t x_average,x_std_dev;
- int32_t y_average,y_std_dev;
- int32_t z_average,z_std_dev;
-
- while(calibrated==0x00)
+ uint8_t data;
+
+ imu_enable_device(ACCEL_ID);
+ imu_spi_start_read(ACCEL_WHO_AM_I,1);
+ imu_wait_op_done();
+ imu_spi_get_data(&data);
+ if(data==ACCEL_ID)
{
- x_average=0;
- y_average=0;
- z_average=0;
- x_std_dev=0;
- y_std_dev=0;
- z_std_dev=0;
- // read 100 samples and compute the standard deviation
- for(i=0;i<IMU_GYRO_CAL_NUM_SAMPLES;i++)
+ ram_data[DARWIN_IMU_STATUS]|=0x01;
+ return 0x01;
+ }
+ else
+ {
+ ram_data[DARWIN_IMU_STATUS]&=0xFE;
+ return 0x00;
+ }
+}
+
+void imu_accel_start(void)
+{
+
+}
+
+void imu_accel_stop(void)
+{
+
+}
+
+void imu_accel_get_config(void)
+{
+ imu_enable_device(ACCEL_ID);
+ imu_spi_start_read(accel_conf_reg,accel_conf_len);
+ imu_wait_op_done();
+ imu_spi_get_data(accel_conf_data);
+}
+
+void imu_accel_set_config(void)
+{
+ accel_conf_data[0]=0x3F;
+ accel_conf_data[3]=0x10;
+
+ imu_enable_device(ACCEL_ID);
+ imu_spi_start_write(accel_conf_reg,accel_conf_data,accel_conf_len);
+ imu_wait_op_done();
+}
+
+void imu_accel_get_data(void)
+{
+ imu_enable_device(ACCEL_ID);
+ imu_spi_start_read(accel_data_reg,accel_data_len);
+}
+
+// gyroscope private functions
+uint8_t imu_gyro_detect(void)
+{
+ uint8_t data;
+
+ imu_enable_device(GYRO_ID);
+ imu_spi_start_read(GYRO_WHO_AM_I,1);
+ imu_wait_op_done();
+ imu_spi_get_data(&data);
+ if(data==GYRO_ID)
+ {
+ ram_data[DARWIN_IMU_STATUS]|=0x02;
+ return 0x01;
+ }
+ else
+ {
+ ram_data[DARWIN_IMU_STATUS]&=0xFD;
+ return 0x00;
+ }
+}
+
+void imu_gyro_start(void)
+{
+
+}
+
+void imu_gyro_stop(void)
+{
+
+}
+
+void imu_gyro_set_config(void)
+{
+ gyro_conf_data[0]=0xFF;// sampling rate = 800 Hz, BW = 110 Hz , all powered up
+ gyro_conf_data[3]=0x20;// Full scale = 2000dps
+
+ imu_enable_device(GYRO_ID);
+ imu_spi_start_write(gyro_conf_reg,gyro_conf_data,gyro_conf_len);
+ imu_wait_op_done();
+}
+
+void imu_gyro_get_config(void)
+{
+ imu_enable_device(GYRO_ID);
+ imu_spi_start_read(gyro_conf_reg,gyro_conf_len);
+ imu_wait_op_done();
+ imu_spi_get_data(gyro_conf_data);
+}
+
+void imu_gyro_get_data(void)
+{
+ imu_enable_device(GYRO_ID);
+ imu_spi_start_read(gyro_data_reg,gyro_data_len);
+}
+
+// interrupt handlers
+void IMU_SPI_IRQHANDLER(void)
+{
+ static uint8_t num_wr=0,num_rd=0;
+
+ /* SPI in Receiver mode */
+ if(SPI_I2S_GetITStatus(IMU_SPI,SPI_I2S_IT_RXNE)==SET)
+ {
+ imu_spi_rd_data[num_rd]=SPI_I2S_ReceiveData(IMU_SPI);
+ num_rd++;
+ if(num_rd >= imu_spi_num_rd_data)
{
- // enable the gyroscope
- GPIO_ResetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
- spi_read_word(GYRO_OUT_X_L,(uint16_t *)&x_samples[i]);
- // disable the gyroscope
- GPIO_SetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
- x_average+=x_samples[i];
- GPIO_ResetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
- spi_read_word(GYRO_OUT_Y_L,(uint16_t *)&y_samples[i]);
- GPIO_SetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
- y_average+=y_samples[i];
- GPIO_ResetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
- spi_read_word(GYRO_OUT_Z_L,(uint16_t *)&z_samples[i]);
- GPIO_SetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
- z_average+=z_samples[i];
-
- delay_ms(1);
+ imu_disable_device();
+ imu_op_state=IMU_DONE;
+ num_rd=0;
+ num_wr=0;
}
- // compute the standard deviation
- for(i=0;i<IMU_GYRO_CAL_NUM_SAMPLES;i++)
+ }
+ /* SPI in Tramitter mode */
+ if(SPI_I2S_GetITStatus(IMU_SPI,SPI_I2S_IT_TXE)==SET)
+ {
+ if(num_wr < imu_spi_num_wr_data)
{
- x_std_dev+=(x_samples[i]-x_average)*(x_samples[i]-x_average);
- y_std_dev+=(y_samples[i]-y_average)*(y_samples[i]-y_average);
- z_std_dev+=(z_samples[i]-z_average)*(z_samples[i]-z_average);
+ SPI_I2S_SendData(IMU_SPI,imu_spi_wr_data[num_wr]);
+ num_wr++;
}
- x_std_dev=sqrt(x_std_dev/IMU_GYRO_CAL_NUM_SAMPLES);
- y_std_dev=sqrt(y_std_dev/IMU_GYRO_CAL_NUM_SAMPLES);
- z_std_dev=sqrt(z_std_dev/IMU_GYRO_CAL_NUM_SAMPLES);
- if((x_std_dev<IMU_GYRO_MAX_STD_DEV) && (y_std_dev<IMU_GYRO_MAX_STD_DEV) && (z_std_dev<IMU_GYRO_MAX_STD_DEV))// calibration complete
+ else
+ SPI_I2S_ITConfig(IMU_SPI,SPI_I2S_IT_TXE,DISABLE);
+ }
+}
+
+void IMU_TIMER_IRQHandler(void)
+{
+ static imu_state_t state=IMU_GET_GYRO;
+
+ TIM_ClearITPendingBit(IMU_TIMER,TIM_IT_Update);
+
+ if(imu_is_op_done())
+ {
+ if(imu_stop_flag==0x01)
{
- gyro_x_center=x_average;
- gyro_y_center=y_average;
- gyro_z_center=z_average;
- calibrated=0x01;
+ // stop the timer to get the imu data
+ TIM_Cmd(IMU_TIMER, DISABLE);
+ TIM_ITConfig(IMU_TIMER, TIM_IT_Update, DISABLE);
+
+ ram_clear_bit(DARWIN_IMU_STATUS,2);
+ imu_stop_flag=0x00;
+ imu_stopped=0x01;
}
else
{
- // make a sound
+ switch(state)
+ {
+ case IMU_GET_ACCEL: imu_spi_get_data(&ram_data[DARWIN_IMU_GYRO_X_L]);
+ imu_accel_get_data();
+ state=IMU_GET_GYRO;
+ break;
+ case IMU_GET_GYRO: imu_spi_get_data(&ram_data[DARWIN_IMU_ACCEL_X_L]);
+ imu_gyro_get_data();
+ state=IMU_GET_ACCEL;
+ break;
+ }
}
}
}
+
// public functions
void imu_init(void)
{
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
- TIM_OCInitTypeDef TIM_OCInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
+ SPI_InitTypeDef SPI_InitStructure;
+ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+ uint8_t i;
GPIO_StructInit(&GPIO_InitStructure);
+ // enable the clocks
+ IMU_SPI_CLK_INIT(IMU_SPI_CLK, ENABLE);
+ RCC_APB2PeriphClockCmd(IMU_MOSI_CLK | IMU_MISO_CLK | IMU_SCK_CLK | IMU_ACCEL_CS_CLK | IMU_GYRO_CS_CLK, ENABLE);
+ /* Enable the IMU timer clock */
+ RCC_APB1PeriphClockCmd(IMU_TIMER_CLK,ENABLE);
+
// initialize the input output ports
- GPIO_InitStructure.GPIO_Pin = PIN_SIG_ACC_CS | PIN_SIG_GYRO_CS;
+ GPIO_InitStructure.GPIO_Pin = IMU_ACCEL_CS_PIN;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_Init(PORT_SPI_CS, &GPIO_InitStructure);
+ GPIO_Init(IMU_ACCEL_CS_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = IMU_GYRO_CS_PIN;
+ GPIO_Init(IMU_GYRO_CS_PORT, &GPIO_InitStructure);
// disable both sensors
- GPIO_SetBits(PORT_SPI_CS,PIN_SIG_GYRO_CS);
- GPIO_SetBits(PORT_SPI_CS,PIN_SIG_ACC_CS);
+ GPIO_SetBits(IMU_ACCEL_CS_PORT,IMU_ACCEL_CS_PIN);
+ GPIO_SetBits(IMU_GYRO_CS_PORT,IMU_GYRO_CS_PIN);
- // initialize the timer
- TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
- TIM_OCStructInit(&TIM_OCInitStructure);
+ // initliaze the SPI peripheral
+ GPIO_StructInit(&GPIO_InitStructure);
+ GPIO_InitStructure.GPIO_Pin = IMU_SCK_PIN;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
+ GPIO_Init(IMU_SCK_PORT, &GPIO_InitStructure);
- // initialize the timer interrupts
- NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority=0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ GPIO_InitStructure.GPIO_Pin = IMU_MOSI_PIN;
+ GPIO_Init(IMU_MOSI_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.GPIO_Pin = IMU_MISO_PIN;
+ GPIO_Init(IMU_MISO_PORT, &GPIO_InitStructure);
+
+ /* initialize variables */
+ imu_spi_num_wr_data=0x00;
+ imu_spi_num_rd_data=0x00;
+ for(i=0;i<32;i++)
+ {
+ imu_spi_rd_data[i]=0x00;
+ imu_spi_wr_data[i]=0x00;
+ }
+ imu_op_state=IMU_DONE;
+ imu_current_device_id=0x00;
+
+ // configure the SPI module
+ SPI_StructInit(&SPI_InitStructure);
+ SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
+ SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
+ SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
+ SPI_InitStructure.SPI_CPOL = SPI_CPOL_High;
+ SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;
+ SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
+ SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8;
+ SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
+ SPI_InitStructure.SPI_CRCPolynomial = 7;
+ SPI_Init(IMU_SPI, &SPI_InitStructure);
+ /* Configure the SPI interrupt priority */
+ NVIC_InitStructure.NVIC_IRQChannel = IMU_SPI_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
+ SPI_I2S_ITConfig(IMU_SPI, SPI_I2S_IT_RXNE , ENABLE);
+
+ /* Enable SPI2 */
+ SPI_Cmd(IMU_SPI, ENABLE);
- TIM_DeInit(TIM4);
- /* Time base configuration */
- TIM_TimeBaseStructure.TIM_Period = 65535;
- TIM_TimeBaseStructure.TIM_Prescaler = 0;
+ /* imu timer configuration */
+ TIM_TimeBaseStructure.TIM_Period = 1000;
+ TIM_TimeBaseStructure.TIM_Prescaler = 72;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
- /* Prescaler configuration */
- TIM_PrescalerConfig(TIM4, 720, TIM_PSCReloadMode_Immediate);// set the timer time base to 100KHz
- TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
- TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
- TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
- /* Output Compare Timing Mode configuration: motion generation period */
- TIM_OCInitStructure.TIM_Pulse = spi_period;// 8 ms period for the motion generation
- TIM_OC1Init(TIM4, &TIM_OCInitStructure);
- TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Disable);
- /* TIM IT enable */
- TIM_ITConfig(TIM4, TIM_IT_CC1, ENABLE);
-
- // initialize the spi module
- spi_init();
-
- // initialize and configure the gyroscope
- imu_init_gyroscope();
- //imu_calibrate_gyroscope();
-
- // initialize and configure the accelerometer
- imu_init_accelerometer();
-
- /* TIM4 enable counter */
- TIM_Cmd(TIM4, ENABLE);
+ TIM_TimeBaseInit(IMU_TIMER,&TIM_TimeBaseStructure);
+
+ NVIC_InitStructure.NVIC_IRQChannel = IMU_TIMER_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+ if(imu_accel_detect())
+ {
+ imu_accel_get_config();
+ imu_accel_set_config();
+ }
+ // detect the sensors
+ if(imu_gyro_detect())
+ {
+ imu_gyro_get_config();
+ imu_gyro_set_config();
+ }
}
-void imu_get_gyroscope(int16_t *x,int16_t *y, int16_t *z)
+void imu_start(void)
{
- ram_read_word(CM730_GYRO_X_L,(uint16_t *)x);
- ram_read_word(CM730_GYRO_Y_L,(uint16_t *)y);
- ram_read_word(CM730_GYRO_Z_L,(uint16_t *)z);
+ // start the accelerometer
+ imu_accel_start();
+ imu_gyro_start();
+
+ // start the timer to get the imu data
+ TIM_Cmd(IMU_TIMER, ENABLE);
+ TIM_ITConfig(IMU_TIMER, TIM_IT_Update, ENABLE);
+ ram_set_bit(DARWIN_IMU_STATUS,2);
}
-void imu_get_accelerometer(int16_t *x, int16_t *y, int16_t *z)
+void imu_stop(void)
{
- ram_read_word(CM730_ACCEL_X_L,(uint16_t *)x);
- ram_read_word(CM730_ACCEL_Y_L,(uint16_t *)y);
- ram_read_word(CM730_ACCEL_Z_L,(uint16_t *)z);
+ imu_stop_flag=0x01;
+ while(!imu_stopped);
+ imu_stopped=0x00;
+ imu_accel_stop();
+ imu_gyro_stop();
}
diff --git a/src/joint_motion.c b/src/joint_motion.c
deleted file mode 100755
index d3910bb91026ae42a570d2b057774460f4c90bef..0000000000000000000000000000000000000000
--- a/src/joint_motion.c
+++ /dev/null
@@ -1,188 +0,0 @@
-#include "joint_motion.h"
-#include "motion_manager.h"
-#include "dyn_servos.h"
-#include "ram.h"
-#include "gpio.h"
-
-// private variables
-int16_t joint_target_angles[JOINT_MAX_NUM_SERVOS];// angle in º/10
-uint16_t joint_target_speeds[JOINT_MAX_NUM_SERVOS];// speed in º/s
-uint16_t joint_target_accels[JOINT_MAX_NUM_SERVOS];// accel in º/s2
-
-int32_t joint_current_angles[JOINT_MAX_NUM_SERVOS];
-int32_t joint_current_speeds[JOINT_MAX_NUM_SERVOS];
-
-int8_t joint_dir[JOINT_MAX_NUM_SERVOS];// direction
-
-uint8_t joint_stop;
-
-// public functions
-void joint_motion_init(void)
-{
- uint8_t i;
-
- /* initialize the internal variables */
- for(i=0;i<JOINT_MAX_NUM_SERVOS;i++)
- {
- // target values
- joint_target_angles[i]=0x0000;
- joint_target_speeds[i]=0x00;
- joint_target_accels[i]=0x00;
- // current values
- joint_current_angles[i]=manager_get_index_angle(i)*1000;
- joint_current_speeds[i]=0;
- // the current angles, speeds and accelerations are in RAM
- joint_dir[i]=0x00;
- }
- ram_write_byte(CM730_JOINT_STATUS,0x00);
- joint_stop=0x00;
-}
-
-void joint_motion_load_angles(uint8_t first,uint8_t num_servos, int16_t *angles)
-{
- int16_t current_angle;
- uint8_t i;
-
- for(i=0;i<num_servos;i++)
- {
- joint_target_angles[first+i]=angles[i];
- current_angle=manager_get_index_angle(first+i);
- if(angles[i]>current_angle) joint_dir[first+i]=1;
- else joint_dir[first+i]=-1;
- }
-}
-
-void joint_motion_load_speeds(uint8_t first,uint8_t num_servos, uint8_t *speeds)
-{
- uint8_t i;
-
- for(i=0;i<num_servos;i++)
- joint_target_speeds[first+i]=speeds[i]*10;// use speed in (º/10)/s
-}
-
-void joint_motion_load_accels(uint8_t first,uint8_t num_servos, uint8_t *accels)
-{
- uint8_t i;
-
- for(i=0;i<num_servos;i++)
- joint_target_accels[first+i]=accels[i]*10;// use acceleration in (º/10)/s2;
-}
-
-void joint_motion_start(void)
-{
- if(joint_motion_is_running()==0x00)// the joints are not moving
- {
- ram_write_byte(CM730_JOINT_STATUS,0x01);
- ram_set_bit(CM730_JOINT_CONTROL,0x00);
- ram_clear_bit(CM730_JOINT_CONTROL,0x01);
- }
-}
-
-void joint_motion_stop(void)
-{
- joint_stop=0x01;
- ram_clear_bit(CM730_JOINT_CONTROL,0x00);
- ram_set_bit(CM730_JOINT_CONTROL,0x01);
-}
-
-uint8_t joint_motion_is_running(void)
-{
- uint8_t running;
-
- ram_read_byte(CM730_JOINT_STATUS,&running);
-
- return running;
-}
-
-// motion manager interface
-void joint_motion_process(void)
-{
- static int32_t target_speed;
- int32_t distance,cmp_speed,cmp_angle;
- uint8_t status=0x00,moving=0x00,id=0;
- uint16_t accel_dist;
-
- ram_read_byte(CM730_JOINT_STATUS,&status);
- if(status&0x01)// start motion
- {
- if(joint_stop==0x01)
- {
- for(id=0;id<manager_get_num_servos();id++)
- if(manager_get_index_module(id)==MM_JOINTS)
- ram_read_word(CM730_SERVO_1_POS_L+id*2,(uint16_t *)&joint_target_angles[id]);
- // keep the speeds and accels for the next motion
- ram_write_byte(CM730_JOINT_STATUS,0x00);
- }
- for(id=0;id<manager_get_num_servos();id++)
- {
- if(manager_get_index_module(id)==MM_JOINTS)// the servo is enabled for this module
- {
- distance=joint_target_angles[id]*1000-joint_current_angles[id];
- if(distance<0) distance=-1*distance;
- if(distance>1000)// still far from the target position
- {
- moving=0x01;
- target_speed=joint_dir[id]*joint_target_speeds[id];
- cmp_speed=joint_current_speeds[id]/1000;
- if(cmp_speed!=target_speed)// it is necessary to change the current speed
- {
- if(cmp_speed>target_speed)
- {
- joint_current_speeds[id]-=(joint_target_accels[id]*manager_get_period());// reduce speed
- if((joint_current_speeds[id]/1000)<target_speed)
- joint_current_speeds[id]=target_speed*1000;
- }
- else
- {
- joint_current_speeds[id]+=(joint_target_accels[id]*manager_get_period());// increase speed
- if((joint_current_speeds[id]/1000)>target_speed)
- joint_current_speeds[id]=target_speed*1000;
- }
- joint_current_angles[id]+=(joint_current_speeds[id]*manager_get_period())/100;
- cmp_angle=joint_target_angles[id]*1000;
- if(joint_dir[id]==1 && joint_current_angles[id]>cmp_angle)
- joint_current_angles[id]=cmp_angle;
- if(joint_dir[id]==-1 && joint_current_angles[id]<cmp_angle)
- joint_current_angles[id]=cmp_angle;
- // set the new angle
- manager_set_index_angle(id,joint_current_angles[id]/1000);
- // set the current speed
- ram_write_byte(CM730_SERVO_1_SPEED+id,(uint8_t)(joint_dir[id]*joint_current_speeds[id]/10000));
- }
- else
- {
- accel_dist=((joint_target_speeds[id]*joint_target_speeds[id]*10000)/(2*joint_target_accels[id]));
- if(distance>accel_dist)// constant speed phase
- {
- joint_current_angles[id]+=(joint_current_speeds[id]*manager_get_period())/100;
- // set the new angle
- manager_set_index_angle(id,joint_current_angles[id]/1000);
- // set the current speed
- ram_write_byte(CM730_SERVO_1_SPEED+id,(uint8_t)(joint_dir[id]*joint_current_speeds[id]/10000));
- }
- else// deceleration phase
- {
- joint_current_speeds[id]-=joint_dir[id]*joint_target_accels[id]*manager_get_period();
- joint_target_speeds[id]=joint_dir[id]*joint_current_speeds[id]/1000;
- joint_current_angles[id]+=(joint_current_speeds[id]*manager_get_period())/100;
- cmp_angle=joint_target_angles[id]*1000;
- if(joint_dir[id]==1 && joint_current_angles[id]>cmp_angle)
- joint_current_angles[id]=cmp_angle;
- if(joint_dir[id]==-1 && joint_current_angles[id]<cmp_angle)
- joint_current_angles[id]=cmp_angle;
- // set the new angle
- manager_set_index_angle(id,joint_current_angles[id]/1000);
- // set the current speed
- ram_write_byte(CM730_SERVO_1_SPEED+id,(uint8_t)(joint_dir[id]*joint_current_speeds[id]/10000));
- }
- }
- }
- else
- joint_current_speeds[id]=0;
- }
- }
- if(moving==0x00)
- ram_write_byte(CM730_JOINT_STATUS,0x00);
- }
-}
-
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 86ae4dbbac16929c34e570ea4d35308a048f8c4b..de103bc1c928266a40ecbcac89d6f1a872a0b6ea 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -1,331 +1,360 @@
#include "motion_manager.h"
-#include "dynamixel_master.h"
+#include "dynamixel_master_uart_dma.h"
#include "dyn_servos.h"
+#include "ram.h"
#include "action.h"
-#include "joint_motion.h"
#include "walking.h"
#include "time.h"
-#include "ram.h"
-#include "gpio.h"
-#include "imu.h"
+
+#define MOTION_TIMER TIM5
+#define MOTION_TIMER_IRQn TIM5_IRQn
+#define MOTION_TIMER_IRQHandler TIM5_IRQHandler
+#define MOTION_TIMER_CLK RCC_APB1Periph_TIM5
// private variables
+__IO uint16_t manager_motion_period;
uint8_t manager_num_servos;
-TServoInfo manager_servos[MAX_NUM_SERVOS];
-
-// Balance control
-uint8_t BALANCE_ENABLE=0x01;
-float BALANCE_KNEE_GAIN=0.3;
-float BALANCE_ANKLE_PITCH_GAIN=0.9;
-float BALANCE_HIP_ROLL_GAIN=0.5;
-float BALANCE_ANKLE_ROLL_GAIN=1.0;
-
-const __IO uint16_t motion_period=800; // 10KH
-const uint8_t manager_period=8;// period of 8 ms for the motion manager
-
-uint8_t manager_enabled;
-
-const int8_t dir[14]={-1,-1,1,1,-1,1,-1,-1,-1,-1,1,1,1,-1};
+TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
// private functions
void manager_send_motion_command(void)
{
- uint8_t servo_ids[MAX_NUM_SERVOS];
- uint8_t data[2048];
- uint16_t value;
- uint8_t i;
+ uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
+ uint8_t data[256];
+ uint8_t i,num=0;
for(i=0;i<manager_num_servos;i++)
{
- servo_ids[i]=manager_servos[i].id;
- value=manager_get_index_value(i);
- data[i*2]=value%256;
- data[i*2+1]=value/256;
+ if(manager_servos[i].enabled)
+ {
+ servo_ids[num]=manager_servos[i].id;
+ data[num*2]=manager_servos[i].current_value%256;
+ data[num*2+1]=manager_servos[i].current_value/256;
+ num++;
+ }
}
- dyn_master_sync_write(manager_num_servos,servo_ids,P_GOAL_POSITION_L,2,data);
+ if(num>0)
+ dyn_master_sync_write(num,servo_ids,P_GOAL_POSITION_L,2,data);
}
-void manager_balance(void)
+inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
{
- int16_t gyro_x;
- int16_t gyro_y;
- int16_t gyro_z;
+ return ((angle+manager_servos[servo_id].center_angle)*manager_servos[servo_id].encoder_resolution)/manager_servos[servo_id].max_angle;
+}
- if(BALANCE_ENABLE == 0x01)
- {
- imu_get_gyroscope(&gyro_x,&gyro_y,&gyro_z);
- // get x,y gyroscope
- manager_servos[R_HIP_YAW].current_value += (int)(((float)(dir[1] * (gyro_x-512))) * BALANCE_HIP_ROLL_GAIN*2.0); // R_HIP_ROLL
- manager_servos[L_HIP_YAW].current_value += (int)(((float)(dir[7] * (gyro_x-512))) * BALANCE_HIP_ROLL_GAIN*2.0); // L_HIP_ROLL
+inline int16_t manager_value_to_angle(uint8_t servo_id,uint16_t value)
+{
+ return (((int16_t)((value*manager_servos[servo_id].max_angle)/manager_servos[servo_id].encoder_resolution))-manager_servos[servo_id].center_angle);
+}
+
+inline uint16_t manager_speed_to_value(uint8_t servo_id,uint16_t speed)
+{
+ if(speed>manager_servos[servo_id].max_speed)
+ speed=manager_servos[servo_id].max_speed;
+ return (speed*3)>>1;
+}
+
+inline uint16_t manager_value_to_speed(uint8_t servo_id,uint16_t value)
+{
+ return (value*2)/3;
+}
+
+void manager_get_current_position(void)
+{
+ uint8_t i;
- manager_servos[R_HIP_PITCH].current_value -= (int)(((float)(dir[3] * (gyro_y-512))) * BALANCE_KNEE_GAIN*2.0); // R_KNEE
- manager_servos[L_HIP_PITCH].current_value -= (int)(((float)(dir[9] * (gyro_y-512))) * BALANCE_KNEE_GAIN*2.0); // L_KNEE
+ for(i=0;i<manager_num_servos;i++)
+ {
+ if(!manager_servos[i].enabled)// servo is disabled
+ {
+ dyn_master_read_word(manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
+ manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
+ }
+ ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
+ ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
+ }
+}
- manager_servos[R_KNEE].current_value -= (int)(((float)(dir[4] * (gyro_y-512))) * BALANCE_ANKLE_PITCH_GAIN*2.0); // R_ANKLE_PITCH
- manager_servos[L_KNEE].current_value -= (int)(((float)(dir[10] * (gyro_y-512))) * BALANCE_ANKLE_PITCH_GAIN*2.0); // L_ANKLE_PITCH
+void manager_get_target_position(void)
+{
+ uint8_t i;
- manager_servos[R_ANKLE_PITCH].current_value -= (int)(((float)(dir[5] * (gyro_x-512))) * BALANCE_ANKLE_ROLL_GAIN*2.0); // R_ANKLE_ROLL
- manager_servos[L_ANKLE_PITCH].current_value -= (int)(((float)(dir[11] * (gyro_x-512))) * BALANCE_ANKLE_ROLL_GAIN*2.0); // L_ANKLE_ROLL
+ for(i=0;i<manager_num_servos;i++)
+ {
+ if(manager_servos[i].enabled)// servo is enabled
+ {
+ if(manager_servos[i].module==MM_ACTION)
+ {
+ manager_servos[i].current_angle=((action_current_angles[i+1])>>9);
+ //>>16 from the action codification, <<7 from the manager codification
+ manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle);
+ }
+ else if(manager_servos[i].module==MM_WALKING)
+ {
+ manager_servos[i].current_angle=walking_current_angles[i+1];
+ manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle);
+ }
+ }
}
}
// interrupt handlers
-void TIM3_IRQHandler(void)
+void MOTION_TIMER_IRQHandler(void)
{
uint16_t capture;
- if (TIM_GetITStatus(TIM3, TIM_IT_CC1) != RESET) // 8 ms
+ if(TIM_GetITStatus(MOTION_TIMER, TIM_IT_CC1)!=RESET)
{
- TIM_ClearITPendingBit(TIM3, TIM_IT_CC1);
+ TIM_ClearITPendingBit(MOTION_TIMER,TIM_IT_CC1);
+ capture = TIM_GetCapture1(MOTION_TIMER);
+ TIM_SetCompare1(MOTION_TIMER, capture + manager_motion_period);
// call the action process
action_process();
// call the joint motion process
- joint_motion_process();
+ // joint_motion_process();
// call the walking process
walking_process();
// balance the robot
- manager_balance();
+ // manager_balance();
+ // get the target angles from all modules
+ manager_get_target_position();
// send the motion commands to the servos
manager_send_motion_command();
- toggle_led4();
- capture = TIM_GetCapture1(TIM3);
- TIM_SetCompare1(TIM3, capture + motion_period);
+ // get the disabled servos position
+// manager_get_current_position();
}
}
// public functions
-void manager_init(void)
+void manager_init(uint16_t period_us)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
- uint8_t servo_ids[MAX_NUM_SERVOS];
- uint8_t i,num=0,error;
+ uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
+ uint8_t i,num=0;
uint16_t model;
-
- dyn_master_power_on();
- delay_ms(500);
- dyn_master_flush();
- // scan the bus for dynamixel devices
- manager_num_servos=0;
+
+ RCC_APB1PeriphClockCmd(MOTION_TIMER_CLK,ENABLE);
+
+ // enable power to the servos
+ dyn_master_enable_power();
+ delay_ms(1000);
+
+ // detect the servos connected
dyn_master_scan(&num,servo_ids);
- for(i=0;i<num;i++)
+ ram_data[DARWIN_MM_NUM_SERVOS]=num;
+ for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
{
- // read the model of the i-th device
- error=dyn_master_read_word(servo_ids[i],P_MODEL_NUMBER_L,&model);
- switch(model)
+ if(i<num)
{
- case SERVO_AX12:
- case SERVO_AX18:
- case SERVO_RX10:
- case SERVO_RX24F:
- case SERVO_RX28:
- case SERVO_RX64: manager_servos[manager_num_servos].max_value=1023;
- manager_servos[manager_num_servos].min_value=0;
- manager_servos[manager_num_servos].center_value=512;
- manager_servos[manager_num_servos].max_angle=1500;
- manager_servos[manager_num_servos].min_angle=-1500;
- break;
- case SERVO_EX106: manager_servos[manager_num_servos].max_value=4095;
- manager_servos[manager_num_servos].min_value=0;
- manager_servos[manager_num_servos].center_value=2048;
- manager_servos[manager_num_servos].max_angle=1255;
- manager_servos[manager_num_servos].min_angle=-1255;
- break;
- case SERVO_MX28: manager_servos[manager_num_servos].max_value=4095;
- manager_servos[manager_num_servos].min_value=0;
- manager_servos[manager_num_servos].center_value=2048;
- manager_servos[manager_num_servos].max_angle=1800;
- manager_servos[manager_num_servos].min_angle=-1800;
- break;
- default: continue;// the device is not a servo
- break;
+ // read the model of the i-th device
+ dyn_master_read_word(servo_ids[i],P_MODEL_NUMBER_L,&model);
+ switch(model)
+ {
+ case SERVO_AX12A: manager_servos[i].encoder_resolution=1023;
+ manager_servos[i].gear_ratio=254;
+ manager_servos[i].max_angle=300<<7;
+ manager_servos[i].center_angle=150<<7;
+ manager_servos[i].max_speed=354;
+ break;
+ case SERVO_AX12W: manager_servos[i].encoder_resolution=1023;
+ manager_servos[i].gear_ratio=32;
+ manager_servos[i].max_angle=300<<7;
+ manager_servos[i].center_angle=150<<7;
+ manager_servos[i].max_speed=2830;
+ break;
+ case SERVO_AX18A: manager_servos[i].encoder_resolution=1023;
+ manager_servos[i].gear_ratio=254;
+ manager_servos[i].max_angle=300<<7;
+ manager_servos[i].center_angle=150<<7;
+ manager_servos[i].max_speed=582;
+ break;
+ case SERVO_MX28: manager_servos[i].encoder_resolution=4095;
+ manager_servos[i].gear_ratio=193;
+ manager_servos[i].max_angle=360<<7;
+ manager_servos[i].center_angle=180<<7;
+ manager_servos[i].max_speed=330;
+ break;
+ case SERVO_RX24F: manager_servos[i].encoder_resolution=1023;
+ manager_servos[i].gear_ratio=193;
+ manager_servos[i].max_angle=300<<7;
+ manager_servos[i].center_angle=150<<7;
+ manager_servos[i].max_speed=756;
+ break;
+ case SERVO_RX28: manager_servos[i].encoder_resolution=1023;
+ manager_servos[i].gear_ratio=193;
+ manager_servos[i].max_angle=300<<7;
+ manager_servos[i].center_angle=150<<7;
+ manager_servos[i].max_speed=402;
+ break;
+ case SERVO_RX64: manager_servos[i].encoder_resolution=1023;
+ manager_servos[i].gear_ratio=200;
+ manager_servos[i].max_angle=300<<7;
+ manager_servos[i].center_angle=150<<7;
+ manager_servos[i].max_speed=294;
+ break;
+ case SERVO_MX64: manager_servos[i].encoder_resolution=4095;
+ manager_servos[i].gear_ratio=200;
+ manager_servos[i].max_angle=360<<7;
+ manager_servos[i].center_angle=180<<7;
+ manager_servos[i].max_speed=378;
+ break;
+ case SERVO_EX106: manager_servos[i].encoder_resolution=1023;
+ manager_servos[i].gear_ratio=184;
+ manager_servos[i].max_angle=250<<7;
+ manager_servos[i].center_angle=125<<7;
+ manager_servos[i].max_speed=414;
+ break;
+ case SERVO_MX106: manager_servos[i].encoder_resolution=4095;
+ manager_servos[i].gear_ratio=225;
+ manager_servos[i].max_angle=360<<7;
+ manager_servos[i].center_angle=180<<7;
+ manager_servos[i].max_speed=270;
+ break;
+ default: break;
+ }
+ manager_servos[i].id=servo_ids[i];
+ manager_servos[i].model=model;
+ manager_servos[i].module=MM_NONE;
+ manager_servos[i].enabled=0x00;
+ // get the servo's current position
+ dyn_master_read_word(manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
+ manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
+ ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
+ ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
+ // set the action current angles
+ action_current_angles[i+1]=manager_servos[i].current_angle<<9;
+ manager_num_servos++;
+ }
+ else
+ {
+ manager_servos[i].id=servo_ids[i];
+ manager_servos[i].model=0x0000;
+ manager_servos[i].module=MM_NONE;
+ manager_servos[i].encoder_resolution=0;
+ manager_servos[i].gear_ratio=0;
+ manager_servos[i].max_angle=0;
+ manager_servos[i].center_angle=0;
+ manager_servos[i].max_speed=0;
+ manager_servos[i].current_value=0;
+ manager_servos[i].current_angle=0;
+ manager_servos[i].enabled=0x00;
}
- manager_servos[manager_num_servos].id=servo_ids[i];
- manager_servos[manager_num_servos].model=model;
- manager_servos[manager_num_servos].module=MM_NONE;
- // set the current position of the servos in RAM
- manager_num_servos++;
}
- manager_read_current_position();
- dyn_master_power_off();
- ram_write_byte(CM730_NUM_SERVOS,manager_num_servos);
- // initialize the timer
- TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
- TIM_OCStructInit(&TIM_OCInitStructure);
+ dyn_master_disable_power();
+ manager_num_servos=num;
// initialize the timer interrupts
- NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority=0;
+ NVIC_InitStructure.NVIC_IRQChannel = MOTION_TIMER_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-
NVIC_Init(&NVIC_InitStructure);
- TIM_DeInit(TIM3);
- /* Time base configuration */
- TIM_TimeBaseStructure.TIM_Period = 65535;
- TIM_TimeBaseStructure.TIM_Prescaler = 0;
+ /* motion timer configuration */
+ TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
+ TIM_TimeBaseStructure.TIM_Prescaler = 36;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
- /* Prescaler configuration */
- TIM_PrescalerConfig(TIM3, 720, TIM_PSCReloadMode_Immediate);// set the timer time base to 100KHz
+ TIM_TimeBaseInit(MOTION_TIMER,&TIM_TimeBaseStructure);
+ TIM_SetClockDivision(MOTION_TIMER,TIM_CKD_DIV1);
+
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
- /* Output Compare Timing Mode configuration: motion generation period */
- TIM_OCInitStructure.TIM_Pulse = motion_period;// 8 ms period for the motion generation
- TIM_OC1Init(TIM3, &TIM_OCInitStructure);
- TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Disable);
- /* the timer is only running when the power is on */
-// TIM_ITConfig(TIM3, TIM_IT_CC1, ENABLE);
-
- // initialize the action module
- action_init();
- // initialize the joint motion module
- joint_motion_init();
- // initialize the walking module
- walking_init();
-
- /* the manager is only enabled when the power is on */
-// TIM_Cmd(TIM3, ENABLE);
-}
+ manager_motion_period=period_us;
+ TIM_OCInitStructure.TIM_Pulse = manager_motion_period;
+ TIM_OC1Init(MOTION_TIMER, &TIM_OCInitStructure);
+ TIM_OC1PreloadConfig(MOTION_TIMER, TIM_OCPreload_Disable);
-void manager_enable(void)
-{
- TIM_ITConfig(TIM3, TIM_IT_CC1, ENABLE);
- TIM_Cmd(TIM3, ENABLE);
- manager_enabled=0x01;
+ ram_clear_bit(DARWIN_MM_STATUS,0);
}
-void manager_disable(void)
+uint16_t manager_get_period(void)
{
- TIM_ITConfig(TIM3, TIM_IT_CC1, DISABLE);
- TIM_Cmd(TIM3, DISABLE);
- manager_enabled=0x00;
+ return manager_motion_period;
}
-uint8_t manager_is_enabled(void)
+void manager_set_period(uint16_t period_us)
{
- return manager_enabled;
+ manager_motion_period=period_us;
}
-inline uint8_t manager_get_num_servos(void)
+inline void manager_enable(void)
{
- return manager_num_servos;
+ TIM_Cmd(MOTION_TIMER, ENABLE);
+ TIM_ITConfig(MOTION_TIMER, TIM_IT_CC1, ENABLE);
+ ram_set_bit(DARWIN_MM_STATUS,0);
}
-inline uint8_t manager_get_period(void)
+inline void manager_disable(void)
{
- return manager_period;
+ TIM_Cmd(MOTION_TIMER, DISABLE);
+ TIM_ITConfig(MOTION_TIMER, TIM_IT_CC1, DISABLE);
+ ram_clear_bit(DARWIN_MM_STATUS,0);
}
-void manager_read_current_position(void)
+inline uint8_t manager_is_enabled(void)
{
- uint16_t value;
- uint8_t i;
-
- for(i=0;i<manager_num_servos;i++)
- {
- // read the current position of the servo
- dyn_master_read_word(manager_servos[i].id,P_PRESENT_POSITION_L,&value);
- manager_set_index_value(i,value);
- }
+ return ram_data[DARWIN_MM_STATUS]&0x01;
}
-void manager_set_servo_angle(uint8_t servo_id, int16_t angle)
-{
- uint8_t i;
-
- for(i=0;i<manager_num_servos;i++)
- {
- if(manager_servos[i].id==servo_id)
- {
- manager_set_index_angle(i,angle);
- break;
- }
- }
-}
-
-void manager_set_index_angle(uint8_t servo_id, int16_t angle)
-{
- manager_servos[servo_id].current_angle=angle;
- manager_servos[servo_id].current_value=(angle*manager_servos[servo_id].max_value)/(manager_servos[servo_id].max_angle-manager_servos[servo_id].min_angle)+manager_servos[servo_id].center_value;
- ram_write_word(CM730_SERVO_1_POS_L+servo_id*2,manager_servos[servo_id].current_angle);
-}
-
-void manager_set_servo_value(uint8_t servo_id, uint16_t value)
-{
- uint8_t i;
-
- for(i=0;i<manager_num_servos;i++)
- {
- if(manager_servos[i].id==servo_id)
- {
- manager_set_index_value(i,value);
- break;
- }
- }
-}
-
-void manager_set_index_value(uint8_t servo_id, uint16_t value)
+inline uint8_t manager_get_num_servos(void)
{
- manager_servos[servo_id].current_value=value;
- manager_servos[servo_id].current_angle=((value-manager_servos[servo_id].center_value)*(manager_servos[servo_id].max_angle-manager_servos[servo_id].min_angle))/manager_servos[servo_id].max_value;
- ram_write_word(CM730_SERVO_1_POS_L+servo_id*2,manager_servos[servo_id].current_angle);
+ return manager_num_servos;
}
void manager_set_module(uint8_t servo_id,TModules module)
{
- manager_servos[servo_id].module=module;
-}
-
-inline int16_t manager_get_servo_angle(uint8_t servo_id)
-{
- uint8_t i;
-
- for(i=0;i<manager_num_servos;i++)
- if(manager_servos[i].id==servo_id)
- return manager_servos[i].current_angle;
-
- return 2048;
+ if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
+ manager_servos[servo_id].module=module;
}
-inline int16_t manager_get_index_angle(uint8_t servo_id)
+inline TModules manager_get_module(uint8_t servo_id)
{
- return manager_servos[servo_id].current_angle;
+ if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
+ return manager_servos[servo_id].module;
+ else
+ return MM_NONE;
}
-inline uint16_t manager_get_servo_value(uint8_t servo_id)
+inline void manager_enable_servo(uint8_t servo_id)
{
- uint8_t i;
-
- for(i=0;i<manager_num_servos;i++)
- if(manager_servos[i].id==servo_id)
- return manager_servos[i].current_value;
-
- return 2048;
+ if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
+ manager_servos[servo_id].enabled=0x01;
}
-inline uint16_t manager_get_index_value(uint8_t servo_id)
+inline void manager_disable_servo(uint8_t servo_id)
{
- return manager_servos[servo_id].current_value;
+ if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
+ manager_servos[servo_id].enabled=0x00;
}
-inline TModules manager_get_servo_module(uint8_t servo_id)
+void manager_disable_servos(void)
{
- uint8_t i;
+ uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
+ uint8_t data[256];
+ uint8_t i,num=0;
for(i=0;i<manager_num_servos;i++)
- if(manager_servos[i].id==servo_id)
- return manager_servos[i].module;
-
- return MM_NONE;
+ {
+ if(!manager_servos[i].enabled)
+ {
+ servo_ids[num]=manager_servos[i].id;
+ data[num]=0x00;
+ num++;
+ }
+ }
+ if(num>0)
+ dyn_master_sync_write(num,servo_ids,P_TORQUE_ENABLE,1,data);
}
-inline TModules manager_get_index_module(uint8_t servo_id)
+inline uint8_t manager_is_servo_enabled(uint8_t servo_id)
{
- return manager_servos[servo_id].module;
+ if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
+ return manager_servos[servo_id].enabled;
+ else
+ return 0x00;
}
diff --git a/src/motion_pages.c b/src/motion_pages.c
index 4e7398ae4e3ffbdee833e3ee92091ffce48c7289..8ec60f7544a64dd1990aecc0c542a739673de558 100755
--- a/src/motion_pages.c
+++ b/src/motion_pages.c
@@ -1,56 +1,2968 @@
#include "motion_pages.h"
-#include "gpio.h"
-
-#define POSE_BASE_ADR (0x08060000UL)
-#define POSE_0_OFFSET (0x00000040UL)
-#define POSE_SIZE (0x00000040UL)
-
-// within a page
-#define POSE_PAGE_SIZE (0x00000200UL)
-#define POSE_PAGE_FAST_FLAG (0x00000010UL)
-#define POSE_PAGE_PLAYCOUNT (0x0000000FUL)
-#define POSE_PAGE_NUM_OF_MOTIONS (0x00000014UL)
-#define POSE_PAGE_MOTION_SPEED (0x00000016UL)
-#define POSE_PAGE_ACCEL_TIME (0x00000018UL)
-#define POSE_PAGE_NEXT_PAGE (0x00000019UL)
-#define POSE_PAGE_EXIT_PAGE (0x0000001AUL)
-
-// Within a pose
-#define POSE_PAUSE_ADR (0x000000003E)
-#define POSE_SPEED_ADR (0x000000003F)
-
-// private functions
-uint8_t check_page_checksum(TPage *page)
+
+unsigned char page_map[MAX_PAGES]={
+ 0,
+ 1,
+ 2,
+ 3,
+ 4,
+ 5,
+ 6,
+ 0,
+ 0,
+ 7,
+ 8,
+ 9,
+ 10,
+ 11,
+ 0,
+ 12,
+ 13,
+ 14,
+ 15,
+ 16,
+ 0,
+ 0,
+ 0,
+ 17,
+ 18,
+ 19,
+ 0,
+ 20,
+ 0,
+ 21,
+ 22,
+ 23,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 24,
+ 25,
+ 0,
+ 26,
+ 27,
+ 28,
+ 29,
+ 30,
+ 31,
+ 32,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 33,
+ 34,
+ 35,
+ 36,
+ 37,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 38,
+ 39,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 40,
+ 41,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 42,
+ 0,
+ 43,
+ 44,
+ 45,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0};
+
+void pages_get_page(uint8_t page_id,TPage *page)
+{
+ uint16_t i=0;
+
+ for(i=0;i<sizeof(TPage);i++)
+ ((uint8_t *)page)[i]=((__IO uint8_t *)&motion_pages[page_map[page_id]])[i];
+}
+
+uint8_t pages_check_checksum(TPage *page)
{
uint8_t checksum=0x00;
- uint16_t i;
+ uint16_t i=0;
- for(i=0;i<POSE_PAGE_SIZE;i++)
+ for(i=0;i<sizeof(TPage);i++)
checksum+=((uint8_t *)page)[i];
-
- if(checksum==0xFF)
- return PAGE_SUCCESS;
+ if(checksum==0x00)
+ return 0x01;
else
- return PAGE_BAD_CHECKSUM;
+ return 0x00;
}
-// public functions
-uint8_t load_page_info(uint8_t page_id,TPage *page)
+void pages_clear_page(TPage *page)
{
- uint8_t *start_address;
- uint16_t i;
-
- start_address=(uint8_t *)(POSE_BASE_ADR+POSE_PAGE_SIZE*page_id);
- for(i=0;i<POSE_PAGE_SIZE;i++)
- (((uint8_t *)page)[i])=(*(start_address+i));
- if(check_page_checksum(page)==PAGE_SUCCESS)
- return PAGE_SUCCESS;
- else
- return PAGE_BAD_CHECKSUM;
+ uint16_t i=0;
+
+ for(i=0;i<sizeof(TPage);i++)
+ ((uint8_t *)page)[i]=0x00;
}
-uint8_t get_offsets(uint16_t *offsets)
+void pages_copy_page(TPage *src,TPage *dst)
{
- return 0x00;
+ uint16_t i=0;
+
+ for(i=0;i<sizeof(TPage);i++)
+ ((uint8_t *)dst)[i]=((uint8_t *)src)[i];
}
+TPage motion_pages[46] __attribute__ ((section (".pages")))=
+{
+ // page 0
+ {
+ {
+ {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x00,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x00,
+ 0x00,
+ 0x00,
+ 0x00,
+ 0x00,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0xf6,
+ {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 1
+ {
+ {
+ {0x69,0x6e,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x01,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x87,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 2
+ {
+ {
+ {0x6f,0x6b,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x05,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x22,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0xfe19,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0xfe19,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 3
+ {
+ {
+ {0x6e,0x6f,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x05,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x23,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 4
+ {
+ {
+ {0x68,0x69,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x04,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x5c,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xdfc6,0x2014,0xf5bf,0x0a1b,0x101d,0xefbe,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xdfc6,0x2014,0xf5bf,0x0a1b,0x101d,0xefbe,0x0000,0x0000,0x0000,0x0000,0xf736,0x08a4,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0x0000,0xf9b4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x18,
+ 0x7d
+ },
+ {
+ {0x6000,0xdfc6,0x2014,0xf5bf,0x0a1b,0x101d,0xefbe,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 5
+ {
+ {
+ {0x3f,0x3f,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x03,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x31,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xe719,0x189c,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf135,0x0bb8,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe719,0x189c,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf135,0x0bb8,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0xe6f4,0x189c,0xf2ac,0x0d09,0x043f,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x0546,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 6
+ {
+ {
+ {0x74,0x61,0x6c,0x6b,0x31,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x07,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x79,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x137b,0x0b6d,0xee92,0x0d2f,0xf7a7,0x0697,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0151,0xffdb,0x0000,0x0096,0xff45,0x0000,0x0000,0xf7cc,0x0697,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x137b,0x0b6d,0xee92,0x0d2f,0xf7a7,0x0697,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0151,0xffdb,0x0000,0x0096,0xff45,0x0000,0x0000,0xf7cc,0x0232,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x137b,0x0b6d,0xee92,0x0d2f,0xf7a7,0x0697,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0151,0xffdb,0x0000,0x0096,0xff45,0x0000,0x0000,0xf7cc,0x0697,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x18,
+ 0x7d
+ },
+ {
+ {0x6000,0xf736,0xfcc7,0xecaa,0x0ecb,0xf75c,0x07e9,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0151,0xffdb,0x0000,0x0096,0xff45,0x0000,0x0000,0x02a3,0x0859,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0xf736,0xfcc7,0xecaa,0x0ecb,0xf75c,0x07e9,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0151,0xffdb,0x0000,0x0096,0xff45,0x0000,0x0000,0x02a3,0x020d,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0xf736,0xfcc7,0xecaa,0x0ecb,0xf75c,0x07e9,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0151,0xffdb,0x0000,0x0096,0xff45,0x0000,0x0000,0x02a3,0x0859,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x18,
+ 0x7d
+ },
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ }
+ }
+ },
+ // page 7
+ {
+ {
+ {0x77,0x61,0x6c,0x6b,0x72,0x65,0x61,0x64,0x79,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x01,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x81,
+ {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 8
+ {
+ {
+ {0x66,0x20,0x75,0x70,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x05,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x9b,
+ {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xe082,0x1bfa,0xf67b,0x0a41,0x21b1,0xdcb3,0xffdb,0xff6a,0x0000,0x0000,0xdf30,0x1f0e,0x1f0e,0xe0f2,0x0b6d,0xf2ac,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x18
+ },
+ {
+ {0x6000,0x0a1b,0xf261,0xf67b,0x0a41,0x2302,0xdc8d,0xffdb,0xff6a,0x0000,0x0000,0xcd9c,0x3138,0x2cad,0xd16b,0x0bdd,0xf216,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x18
+ },
+ {
+ {0x6000,0x1042,0xef98,0xedfc,0x1204,0xe05c,0x1de2,0xffdb,0xff6a,0x0000,0x0000,0xcf5e,0x2d1e,0x4155,0xbdf0,0x23be,0xd99f,0x0000,0x0000,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x63
+ },
+ {
+ {0x6000,0x0697,0xf6eb,0xedfc,0x1204,0xe05c,0x1de2,0xffdb,0xff6a,0x0000,0x0000,0xcf5e,0x2f50,0x3e41,0xc093,0x197d,0xe57d,0x0000,0x0000,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 9
+ {
+ {
+ {0x62,0x20,0x75,0x70,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x06,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0xcd,
+ {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xb677,0x4963,0xe997,0x13c6,0x1c90,0xe325,0xffdb,0xff6a,0x0000,0x0000,0xd2e2,0x2c3d,0x331f,0xcad4,0x1194,0xec85,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x18
+ },
+ {
+ {0x6000,0xb677,0x4963,0xe997,0x13c6,0xff45,0x0070,0xffdb,0xff6a,0x0000,0x0000,0xd2e2,0x2c3d,0x331f,0xcad4,0x101d,0xee21,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xbbe3,0x4099,0xf67b,0x0a41,0xe5ed,0x1ace,0xffdb,0xff6a,0x0000,0x0000,0x0f61,0xef98,0x1d4c,0xe2b4,0x0000,0x0000,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x18,
+ 0x31
+ },
+ {
+ {0x6000,0xd3e9,0x25a5,0xecf5,0x0f3c,0xdbd2,0x2848,0xffdb,0xff6a,0x0000,0x0000,0x0fac,0xef4d,0x3507,0xc982,0x2742,0xd7b8,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x31,
+ 0x4a
+ },
+ {
+ {0x6000,0x0ea6,0xf15a,0xecf5,0x0f3c,0xdbd2,0x2848,0xffdb,0xff6a,0x0000,0x0000,0x0fac,0xef4d,0x2db4,0xd0d5,0x2742,0xd7b8,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x18,
+ 0x63
+ },
+ {
+ {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page a
+ {
+ {
+ {0x72,0x6b,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x07,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x9d,
+ {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x44,0x44,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xe890,0x13a1,0xf6c6,0x08ca,0x0e5b,0xf135,0xffb5,0x0025,0x0000,0x0000,0xec14,0x13ec,0x1644,0xe9e2,0x0a41,0xf448,0xffb5,0xff45,0x0000,0x137b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe3e0,0x1068,0xf6c6,0x0a8c,0x0ecb,0xf15a,0x0025,0x0025,0x0070,0xffdb,0xea2d,0x161e,0x1c20,0xe42b,0x0e10,0xf23b,0x07c3,0x04fb,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x18
+ },
+ {
+ {0x6000,0xe3e0,0x1068,0xf6c6,0x0a8c,0x0ecb,0xf15a,0x0025,0x0025,0x035e,0xfe64,0xe50c,0x19c8,0x2ac6,0xe42b,0x1563,0xf23b,0x07c3,0x04fb,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x09
+ },
+ {
+ {0x6000,0xe0f2,0x079e,0xf54f,0x0a8c,0x0025,0xe719,0x0025,0x0025,0x035e,0xfe64,0xe0a7,0x1563,0x0ef1,0xe42b,0xf6a0,0xf2f7,0x04d5,0x0384,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x12,
+ 0x09
+ },
+ {
+ {0x6000,0xe845,0x14cd,0xf6c6,0x0a8c,0x0ecb,0xf15a,0x0025,0x0025,0x035e,0xfe64,0xe21e,0x16da,0x2ac6,0xe42b,0x1563,0xf23b,0x04d5,0x043f,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x09
+ },
+ {
+ {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500},
+ 0x00,
+ 0x09
+ },
+ {
+ {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500},
+ 0x00,
+ 0x3d
+ }
+ }
+ },
+ // page b
+ {
+ {
+ {0x6c,0x6b,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x07,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0xbb,
+ {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x44,0x44,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xe890,0x13a1,0xf6c6,0x08ca,0x0e5b,0xf135,0xffb5,0x0025,0x0000,0x0000,0xec14,0x13ec,0x1644,0xe9e2,0x0a41,0xf448,0xffb5,0xff45,0x0000,0x137b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xef73,0x1bfa,0xf54f,0x0915,0x0e80,0xf10f,0xffb5,0xffb5,0x0000,0xff6a,0xe9bc,0x15ae,0x1baf,0xe3bb,0x0d9f,0xf1cb,0xfae0,0xf817,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x18
+ },
+ {
+ {0x6000,0xef73,0x1bfa,0xf54f,0x0915,0x0e80,0xf10f,0xffb5,0xffb5,0x0177,0xfc7c,0xe613,0x1ace,0x1baf,0xd515,0x0d9f,0xea78,0xfae0,0xf817,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x09
+ },
+ {
+ {0x6000,0xf83d,0x1ee8,0xf54f,0x0a8c,0x18c1,0xffb5,0xffb5,0xffb5,0x0177,0xfc7c,0xea78,0x1f33,0x1baf,0xf0ea,0x0ce4,0x093a,0xfc57,0xfb05,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x12,
+ 0x09
+ },
+ {
+ {0x6000,0xeb0e,0x1795,0xf54f,0x0915,0x0e80,0xf10f,0xffb5,0xffb5,0x0177,0xfc7c,0xe901,0x1dbc,0x1baf,0xd515,0x0d9f,0xea78,0xfb9b,0xfb05,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x09
+ },
+ {
+ {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500},
+ 0x00,
+ 0x09
+ },
+ {
+ {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500},
+ 0x00,
+ 0x3d
+ }
+ }
+ },
+ // page c
+ {
+ {
+ {0x73,0x69,0x74,0x20,0x64,0x6f,0x77,0x6e,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x01,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x81,
+ {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xe7fa,0x14a7,0xf6a0,0x0a66,0x0ea6,0xf135,0xffdb,0xff6a,0x0070,0xffdb,0xde2a,0x20f5,0x4074,0xbf1c,0x2302,0xdc8d,0x0151,0xff90,0x0025,0x0591,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page d
+ {
+ {
+ {0x73,0x74,0x61,0x6e,0x64,0x20,0x75,0x70,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x01,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x26,
+ {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page e
+ {
+ {
+ {0x6d,0x75,0x6c,0x31,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x07,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x12,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x40,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x079e,0xf9ff,0xee92,0x11b9,0xe926,0x09d0,0xffdb,0xff6a,0x0000,0x0000,0xdcd8,0x226c,0x4074,0xbf67,0x2328,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x137b,0xea9d,0xecaa,0x14cd,0xffb5,0xff45,0xffdb,0xff6a,0x0000,0x0000,0xcf13,0x2f2b,0x30ed,0xce7d,0x2328,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x27d8,0xd6b1,0xecaa,0x14a7,0x1c6b,0xe13d,0xffdb,0xff6a,0x0000,0x0000,0xcf39,0x2f50,0xff45,0xffb5,0x2328,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x4ada,0xb500,0xecaa,0x14a7,0x1c6b,0xe13d,0xffdb,0xff6a,0x0000,0x0000,0xcf39,0x2f50,0xff45,0xffb5,0x2328,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x4ada,0xb500,0x129a,0xec14,0xdfa1,0x24c4,0xffdb,0xff6a,0x0000,0x0000,0xcf39,0x2f50,0xff45,0xffb5,0x2328,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x0915,0xf493,0x168f,0xe78a,0xd0b0,0x2c3d,0xffdb,0xff6a,0x0000,0x0000,0xcf39,0x2f50,0xff45,0xffb5,0x2328,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x027d,0xfda8,0xe901,0x13ec,0x16ff,0xea07,0xffdb,0xff6a,0x0000,0x0000,0xcf39,0x2f50,0xff45,0xffb5,0x2328,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ }
+ }
+ },
+ // page f
+ {
+ {
+ {0x6d,0x75,0x6c,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x07,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x13,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0xd3,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x027d,0xfda8,0xe901,0x13ec,0x16ff,0xea07,0xffdb,0xff6a,0x0000,0x0000,0x0f87,0x2f50,0x27b2,0xffb5,0x1e07,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x0e5b,0xf0ea,0xe901,0x13ec,0x16ff,0xea07,0xffdb,0xff6a,0x0000,0x0000,0x0f61,0xef73,0x0dea,0xf054,0xea07,0x12c0,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x1a13,0xe532,0xe901,0x13ec,0x0cbe,0xf448,0xffdb,0xff6a,0x0000,0x0000,0xfdce,0x0106,0x00bb,0xfefa,0xea07,0x12c0,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x1a13,0xe532,0xe901,0x13ec,0x0cbe,0xf448,0xffdb,0xff6a,0x1bfa,0xe28f,0xfdce,0x0106,0x00bb,0xfefa,0xea07,0x12c0,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x1a13,0xe532,0xe901,0x13ec,0x0cbe,0xf448,0xffdb,0xff6a,0x0000,0x0000,0xfdce,0x0106,0x00bb,0xfefa,0xea07,0x12c0,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x1a13,0xe532,0xe901,0x13ec,0x0cbe,0xf448,0xffdb,0xff6a,0x1bfa,0xe28f,0xfdce,0x0106,0x00bb,0xfefa,0xea07,0x12c0,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x1a13,0xe532,0xe901,0x13ec,0x0cbe,0xf448,0xffdb,0xff6a,0x0000,0x0000,0xfdce,0x0106,0x00bb,0xfefa,0xea07,0x12c0,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ }
+ }
+ },
+ // page 10
+ {
+ {
+ {0x6d,0x75,0x6c,0x33,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x06,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x59,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x027d,0xfda8,0xe901,0x13ec,0x16ff,0xea07,0xffdb,0xff6a,0x0000,0x0000,0x0f87,0x2f50,0x27b2,0xffb5,0x1e07,0xeb59,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x027d,0xfda8,0xe901,0x13ec,0x16ff,0xea07,0xffdb,0xff6a,0x0000,0x0000,0xcf39,0x2f50,0xff45,0xffb5,0x11df,0xebc9,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x02c8,0xfe19,0xecaa,0x11df,0x20f5,0xdddf,0xffdb,0xff6a,0x0000,0x0000,0xcf39,0x2f50,0x1bfa,0xe42b,0x2328,0xdcb3,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x1042,0xef98,0xedfc,0x1204,0xe05c,0x1de2,0xffdb,0xff6a,0x0000,0x0000,0xcf5e,0x2d1e,0x4155,0xbdf0,0x23be,0xd99f,0x0000,0x0000,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x0697,0xf6eb,0xedfc,0x1204,0xe05c,0x1de2,0xffdb,0xff6a,0x0000,0x0000,0xcf5e,0x2f50,0x412f,0xbda5,0x197d,0xe57d,0x0000,0x0000,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x1500,0xe7d5,0x14a7,0xf711,0x08ca,0x0ea6,0xf135,0x0000,0x0000,0x0025,0xffdb,0xef02,0x10fe,0x1a38,0xe5c8,0x0fac,0xf054,0x004b,0xffb5,0x0000,0x13ec,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500,0x1500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 11
+ {
+ {
+ {0x64,0x31,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x04,
+ 0x00,
+ 0x15,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x2a,
+ {0x55,0x66,0x66,0x66,0x66,0x66,0x66,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x2e6f,0x0e80,0xf574,0x0d2f,0xf54f,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0465,0x0859,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0xf31c,0x0e80,0xf054,0x0d2f,0x211b,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfb9b,0x0465,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0x0465,0xfc57,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x4a
+ },
+ {
+ {0x6000,0xffdb,0x0e80,0xf574,0x0d2f,0x130b,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0465,0xff90,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 12
+ {
+ {
+ {0x64,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x05,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x19,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x82,
+ {0x55,0x66,0x66,0x66,0x66,0x66,0x66,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x3be9,0xc543,0xf75c,0x0ea6,0xf15a,0x0a1b,0xf8ad,0xf8ad,0x0000,0x0000,0x0000,0xfd12,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0xfa
+ },
+ {
+ {0x6000,0x3be9,0xc543,0xe163,0x249f,0xf15a,0x0a1b,0xf8ad,0xf8ad,0x0000,0x0000,0x0000,0xfd12,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x3be9,0xc543,0xf75c,0x0ea6,0xf15a,0x0a1b,0xf8ad,0xf8ad,0x0000,0x0000,0x0000,0xfd12,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x3be9,0xc543,0xe163,0x249f,0xf15a,0x0a1b,0xf8ad,0xf8ad,0x0000,0x0000,0x0000,0xfd12,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x3be9,0xc543,0xf75c,0x0ea6,0xf15a,0x0a1b,0xf8ad,0xf8ad,0x0000,0x0000,0x0000,0xfd12,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 13
+ {
+ {
+ {0x64,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x06,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x67,
+ {0x55,0x66,0x66,0x66,0x66,0x66,0x66,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x3be9,0xc543,0xf009,0x0753,0xf15a,0x0a1b,0x0753,0x0753,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0xba
+ },
+ {
+ {0x6000,0x3be9,0xc543,0xda10,0x1d4c,0xf15a,0x0a1b,0x0753,0x0753,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x3be9,0xc543,0xf009,0x0753,0xf15a,0x0a1b,0x0753,0x0753,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x3be9,0xc543,0xda10,0x1d4c,0xf15a,0x0a1b,0x0753,0x0753,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x3be9,0xc543,0xf009,0x0753,0xf15a,0x0a1b,0x0753,0x0753,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x16ff,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0xba
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 14
+ {
+ {
+ {0x64,0x33,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x05,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x89,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x3183,0x18c1,0x004b,0x0ce4,0x1a5e,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfa24,0x05dc,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0xf2f7,0x004b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x1d71,0x18c1,0xf31c,0x0ce4,0x1e52,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfa24,0x05dc,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0xf969,0xf83d,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x3183,0x18c1,0x004b,0x0ce4,0x1a5e,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfa24,0x05dc,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0xf2f7,0x004b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x1d71,0x18c1,0xf31c,0x0ce4,0x1e52,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfa24,0x05dc,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0xf969,0xf83d,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 15
+ {
+ {
+ {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x05,
+ 0x00,
+ 0x15,
+ 0x00,
+ 0x20,
+ 0x1e,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0xc4,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 16
+ {
+ {
+ {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x05,
+ 0x00,
+ 0x15,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x78,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 17
+ {
+ {
+ {0x64,0x34,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x06,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x24,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x3112,0xcea3,0x05b6,0xf67b,0xd1b6,0x2dd9,0x0000,0x0000,0x0000,0x0000,0xfe64,0x0177,0x0753,0xf8ad,0x0520,0xfae0,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x3865,0xd5f6,0x218b,0xdec0,0xe05c,0x2dd9,0x0000,0x0000,0x0000,0x0000,0x0e80,0xf2d1,0x0a41,0xf5bf,0x0c73,0xf38d,0x0000,0x0000,0x0000,0xf8f8,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x3865,0xd5f6,0x05b6,0xf67b,0xe05c,0x2dd9,0x0000,0x0000,0x0000,0x0000,0xf135,0x101d,0x0a41,0xf5bf,0x0232,0xfdce,0x0000,0x0000,0x0000,0x0dea,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x29bf,0xc750,0x218b,0xdec0,0xd1b6,0x1f33,0x0000,0x0000,0x0000,0x0000,0x0e80,0xf2d1,0x0a41,0xf5bf,0x0c73,0xf38d,0x0000,0x0000,0x0000,0xf8f8,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x29bf,0xc750,0x05b6,0xf67b,0xd1b6,0x1f33,0x0000,0x0000,0x0000,0x0000,0xf135,0x101d,0x0a41,0xf5bf,0x00bb,0xff45,0x0000,0x0000,0x0000,0x0dea,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 18
+ {
+ {
+ {0x64,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x05,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x27,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x15,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x3be9,0x09d0,0xf75c,0x0f87,0xf15a,0x09f6,0xf8ad,0xf8ad,0x0000,0x0000,0xfd12,0x0000,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0xfa
+ },
+ {
+ {0x6000,0x3be9,0x09d0,0xe8b6,0x0f87,0xe971,0x09f6,0xf8ad,0xf8ad,0x0000,0x0000,0xfd12,0x0000,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x3be9,0x09d0,0xf75c,0x0f87,0xf15a,0x09f6,0xf8ad,0xf8ad,0x0000,0x0000,0xfd12,0x0000,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x3be9,0x09d0,0xe8b6,0x0f87,0xe971,0x09f6,0xf8ad,0xf8ad,0x0000,0x0000,0xfd12,0x0000,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x3be9,0x09d0,0xf75c,0x0f87,0xf15a,0x09f6,0xf8ad,0xf8ad,0x0000,0x0000,0xfd12,0x0000,0x0000,0x0000,0xff45,0xfdce,0x0000,0x0000,0xf448,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 19
+ {
+ {
+ {0x64,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x06,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0xce,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x3be9,0x09d0,0xe8b6,0x0f87,0xf15a,0x09f6,0x0753,0x0753,0x0000,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x3be9,0x09d0,0xda10,0x0f87,0xea2d,0x09f6,0x0753,0x0753,0x0000,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x3be9,0x09d0,0xe8b6,0x0f87,0xf15a,0x09f6,0x0753,0x0753,0x0000,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x3be9,0x09d0,0xda10,0x0f87,0xea2d,0x09f6,0x0753,0x0753,0x0000,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x3be9,0x09d0,0xe8b6,0x0f87,0xf15a,0x09f6,0x0753,0x0753,0x0000,0x0000,0x0000,0x02ee,0x0000,0x0000,0x0232,0x00bb,0x0000,0x0000,0x0bb8,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0xba
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 1a
+ {
+ {
+ {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x03,
+ 0x00,
+ 0x15,
+ 0x00,
+ 0x20,
+ 0x2a,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x44,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xe5a2,0x1a38,0xf2d1,0x0d09,0x0753,0xf888,0x0000,0x0000,0x0000,0x0000,0xf8ad,0x0753,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0x0000,0xfe19,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x63
+ },
+ {
+ {0x6000,0xe5a2,0x1a38,0xf2d1,0x0d09,0x0753,0xf888,0x0000,0x0000,0x0000,0x0000,0xfa24,0x05dc,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0xfb9b,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe5a2,0x1a38,0xf2d1,0x0d09,0x0753,0xf888,0x0000,0x0000,0x0000,0x0000,0xfb9b,0x0465,0x02ee,0xfd12,0x00bb,0xff45,0x0000,0x0000,0xfb9b,0x0859,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 1b
+ {
+ {
+ {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x05,
+ 0x00,
+ 0x15,
+ 0x00,
+ 0x20,
+ 0x2b,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0xb1,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xf54f,0x06e2,0x08a4,0x0a66,0x20aa,0x03a9,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x01c2,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0xf54f,0x06e2,0x08a4,0x0a66,0x20aa,0x03a9,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x01c2,0x027d,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xf54f,0x06e2,0x08a4,0x0a66,0x20aa,0x03a9,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x01c2,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xf54f,0x06e2,0x08a4,0x0a66,0x20aa,0x03a9,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x01c2,0x027d,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xf54f,0x06e2,0x08a4,0x0a66,0x20aa,0x03a9,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x01c2,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 1c
+ {
+ {
+ {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x05,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x2c,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0xab,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 1d
+ {
+ {
+ {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x05,
+ 0x00,
+ 0x15,
+ 0x00,
+ 0x20,
+ 0x2d,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x4b,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xf6eb,0xfd12,0xef98,0x0d7a,0xfc57,0x0384,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 1e
+ {
+ {
+ {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x07,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x2e,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0xe8,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x0ad7,0xfa4a,0xf4de,0x0859,0xff1f,0x064c,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf5bf,0x0859,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x0ad7,0xfa4a,0xf4de,0x0859,0xff1f,0x064c,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf5bf,0x04b0,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0ad7,0xfa4a,0xf4de,0x0859,0xff1f,0x064c,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf5bf,0x0859,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0ad7,0xfa4a,0xf4de,0x0859,0xff1f,0x064c,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf5bf,0x04b0,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0ad7,0xfa4a,0xf4de,0x0859,0xff1f,0x064c,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf5bf,0x0859,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0ad7,0xfa4a,0xf4de,0x0859,0xff1f,0x064c,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf5bf,0x04b0,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0ad7,0xfa4a,0xf4de,0x0859,0xff1f,0x064c,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf5bf,0x0859,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ }
+ }
+ },
+ // page 1f
+ {
+ {
+ {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x05,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x2f,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0xe6,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x0313,0x041a,0xf2f7,0x0d2f,0xedd6,0x0c28,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x0313,0x041a,0xf2f7,0x0d2f,0xedd6,0x0c28,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0313,0x041a,0xf2f7,0x0d2f,0xedd6,0x0c28,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0313,0x041a,0xf2f7,0x0d2f,0xedd6,0x0c28,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0313,0x041a,0xf2f7,0x0d2f,0xedd6,0x0c28,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 20
+ {
+ {
+ {0x74,0x61,0x6c,0x6b,0x32,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x06,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x96,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x03f4,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x0601,0x0e35,0xf009,0x0cbe,0xfcc7,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x06e2,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 21
+ {
+ {
+ {0x69,0x6e,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x02,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x37,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x38,
+ {0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x3219,0xcbda,0xf2d1,0x0d7a,0x2085,0xdcfe,0xffb5,0x0000,0xffb5,0x0025,0x0106,0xfe3e,0xff90,0x0025,0x0106,0xfe64,0x0025,0xff45,0xf8ad,0x0c4e,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x4210,0xbc79,0xf2f7,0x0dc5,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 22
+ {
+ {
+ {0x69,0x6e,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x06,
+ 0x00,
+ 0x10,
+ 0x00,
+ 0x20,
+ 0x38,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0xaf,
+ {0x55,0x77,0x77,0x77,0x77,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x4210,0xbc79,0xe1ae,0x1c90,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xf8ad,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xfa24,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x4210,0xbc79,0xe1ae,0x1c90,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xfb9b,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xfd12,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x4210,0xbc79,0xe1ae,0x1c90,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xfe89,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 23
+ {
+ {
+ {0x69,0x6e,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x06,
+ 0x00,
+ 0x10,
+ 0x00,
+ 0x20,
+ 0x3a,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0xc2,
+ {0x55,0x66,0x66,0x66,0x66,0x66,0x66,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x4210,0xbc79,0xe188,0x1d26,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0177,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x02ee,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x4210,0xbc79,0xe188,0x1d26,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0465,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x05dc,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x4210,0xbc79,0xe188,0x1d26,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0753,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x05dc,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 24
+ {
+ {
+ {0x69,0x6e,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x06,
+ 0x00,
+ 0x10,
+ 0x00,
+ 0x20,
+ 0x3a,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x04,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x66,0x66,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x4210,0xbc79,0xe188,0x1c6b,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0465,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x02ee,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x4210,0xbc79,0xe188,0x1c6b,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0177,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x4210,0xbc79,0xe188,0x1c6b,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xfe89,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0x4210,0xbc79,0xf83d,0x0591,0xe49c,0x19ed,0x0000,0x0000,0x0000,0x0000,0x0177,0xfe89,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0xfd12,0x0cbe,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 25
+ {
+ {
+ {0x69,0x6e,0x74,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x01,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x06,
+ {0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xe719,0x18c1,0xf2d1,0x0d09,0x0465,0xfb76,0x0000,0x0000,0x0000,0x0000,0xfe89,0x0177,0x0000,0x0000,0x00bb,0xff45,0x0000,0x0000,0x0000,0x056b,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0xba
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 26
+ {
+ {
+ {0x72,0x50,0x41,0x53,0x53,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x07,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x49,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x44,0x44,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xdc8d,0x1068,0xf6c6,0x0a8c,0x0025,0xf15a,0x0025,0x0025,0x0070,0xffdb,0xe971,0x16da,0x1c20,0xe42b,0x0e10,0xf23b,0x0ab1,0x043f,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x18
+ },
+ {
+ {0x6000,0xd828,0x1068,0xf9b4,0x0a8c,0x0025,0xf15a,0x0ecb,0x0025,0x035e,0xfe64,0xe50c,0x19c8,0x2ac6,0xe42b,0x1563,0xf23b,0x07c3,0x043f,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x09
+ },
+ {
+ {0x6000,0xd99f,0x079e,0xf54f,0x0a8c,0x0025,0xe719,0x11b9,0x0025,0xfefa,0xfe64,0xe0a7,0x1563,0x0ef1,0xe42b,0xf6a0,0xf2f7,0x04d5,0x043f,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x09
+ },
+ {
+ {0x6000,0xd24c,0x079e,0xf54f,0x0a8c,0x0025,0xe719,0x0d54,0x0025,0xed66,0xfe64,0xe0a7,0x1563,0x0ef1,0xe42b,0xf6a0,0xf2f7,0x04d5,0x043f,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x0b,
+ 0x09
+ },
+ {
+ {0x6000,0xd99f,0x079e,0xf54f,0x0a8c,0x0025,0xe719,0x11b9,0x0025,0xfefa,0xfe64,0xe0a7,0x1563,0x0ef1,0xe42b,0xf6a0,0xf2f7,0x04d5,0x043f,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x09
+ },
+ {
+ {0x6000,0xe845,0x14cd,0xf6c6,0x0a8c,0x0ecb,0xf15a,0x0025,0x0025,0x01e7,0xfe64,0xe21e,0x16da,0x2ac6,0xe42b,0x1563,0xf23b,0x04d5,0x0384,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x18
+ },
+ {
+ {0x6000,0xe7fa,0x14cd,0xf6c6,0x0a8c,0x0ecb,0xf15a,0x0025,0x0025,0x0070,0xffdb,0xe971,0x16da,0x1c20,0xe42b,0x0e10,0xf23b,0x0070,0xffdb,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x09
+ }
+ }
+ },
+ // page 27
+ {
+ {
+ {0x6c,0x50,0x41,0x53,0x53,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x07,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x3c,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x44,0x44,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xef73,0x234d,0xf54f,0x0915,0x0e80,0xffb5,0xffb5,0xffb5,0x0000,0xff6a,0xe901,0x1669,0x1baf,0xe3bb,0x0d9f,0xf1cb,0xfb9b,0xf529,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x18
+ },
+ {
+ {0x6000,0xef73,0x27b2,0xf54f,0x0627,0x0e80,0xffb5,0xffb5,0xf10f,0x0177,0xfc7c,0xe613,0x1ace,0x1baf,0xd515,0x0d9f,0xea78,0xfb9b,0xf817,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x09
+ },
+ {
+ {0x6000,0xf83d,0x263b,0xf54f,0x0a8c,0x18c1,0xffb5,0xffb5,0xee21,0x0177,0x00e1,0xea78,0x1f33,0x1baf,0xf0ea,0x0ce4,0x093a,0xfb9b,0xfb05,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x09
+ },
+ {
+ {0x6000,0xf83d,0x2d8e,0xf54f,0x0a8c,0x18c1,0xffb5,0xffb5,0xf286,0x0177,0x1275,0xea78,0x1f33,0x1baf,0xf0ea,0x0ce4,0x093a,0xfb9b,0xfb05,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x0b,
+ 0x09
+ },
+ {
+ {0x6000,0xf83d,0x263b,0xf54f,0x0a8c,0x18c1,0xffb5,0xffb5,0xee21,0x0177,0x00e1,0xea78,0x1f33,0x1baf,0xf0ea,0x0ce4,0x093a,0xfb9b,0xfb05,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x09
+ },
+ {
+ {0x6000,0xeb0e,0x1795,0xf54f,0x0915,0x0e80,0xf10f,0xffb5,0xffb5,0x0177,0xfdf3,0xe901,0x1dbc,0x1baf,0xd515,0x0d9f,0xea78,0xfc57,0xfb05,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x18
+ },
+ {
+ {0x6000,0xe7fa,0x14cd,0xf6c6,0x0a8c,0x0ecb,0xf15a,0x0025,0x0025,0x0070,0xffdb,0xe971,0x16da,0x1c20,0xe42b,0x0e10,0xf23b,0x0070,0xffdb,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x09
+ }
+ }
+ },
+ // page 28
+ {
+ {
+ {0x6c,0x69,0x65,0x20,0x64,0x6f,0x77,0x6e,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x04,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x63,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0x15f9,0xeba4,0xea78,0x13ec,0xdb3c,0x1e07,0x0025,0x0025,0x0070,0xffdb,0xd24c,0x2dd9,0x3f93,0xbe86,0x263b,0xd909,0x0070,0xffdb,0x0025,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0x0ff7,0xedd6,0xea78,0x1411,0x20aa,0xdd23,0x0025,0x0025,0x0070,0xffdb,0xeb7e,0x12c0,0x0ad7,0xf59a,0x2661,0xd899,0x0070,0xffdb,0x0025,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0xe50c,0x1932,0xea78,0x1411,0x20aa,0xdd23,0x0025,0x0025,0x0070,0xffdb,0xeb7e,0x12c0,0x0ad7,0xf59a,0x2661,0xd899,0x0070,0xffdb,0x0025,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0xe7fa,0x14cd,0xf6c6,0x0a8c,0x0ecb,0xf15a,0x0025,0x0025,0x0070,0xffdb,0xe971,0x16da,0x1c20,0xe42b,0x0e10,0xf23b,0x0070,0xffdb,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 29
+ {
+ {
+ {0x6c,0x69,0x65,0x20,0x75,0x70,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x01,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x03,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0xd8,
+ {0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xd3e9,0x25a5,0xecf5,0x0f3c,0xdbd2,0x2848,0xffdb,0xff6a,0x0000,0x0000,0x0fac,0xef4d,0x3219,0xcc70,0x2742,0xd7b8,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0xb677,0x4963,0xe997,0x0ef1,0x0960,0xf2d1,0xffdb,0xff6a,0x0000,0x0000,0x0f16,0xf054,0x101d,0xedfc,0xee6c,0x0fac,0x00bb,0xff90,0x0000,0x1411,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x6000,0xe7fa,0x14cd,0xf6c6,0x0a8c,0x0ecb,0xf15a,0x0025,0x0025,0x0070,0xffdb,0xe971,0x16da,0x1c20,0xe42b,0x0e10,0xf23b,0x0070,0xffdb,0x0025,0x1437,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x7d
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 2a
+ {
+ {
+ {0x73,0x69,0x74,0x20,0x64,0x6f,0x77,0x6e,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x06,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x02,
+ 0x00,
+ 0x0c,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0xda,
+ {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xe7fa,0x14a7,0xf6a0,0x0a66,0x0ea6,0xf135,0xffdb,0xff6a,0x0070,0xffdb,0xe57d,0x19a2,0x2904,0xd68c,0x15d3,0xe9bc,0x0151,0xff90,0x0025,0x0591,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe890,0x13a1,0xf6c6,0x08ca,0x0e5b,0xf135,0xffb5,0x0025,0x0000,0x0000,0xec14,0x13ec,0x1644,0xe9e2,0x093a,0xf54f,0xffb5,0xff45,0x0000,0x093a,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 2b
+ {
+ {
+ {0x73,0x69,0x74,0x20,0x64,0x6f,0x77,0x6e,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x04,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x02,
+ 0x00,
+ 0x0c,
+ 0x00,
+ 0x20,
+ 0xf0,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0xeb,
+ {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xe7fa,0x14a7,0xf6a0,0x0a66,0x0ea6,0xf135,0xffdb,0xff6a,0x0070,0xffdb,0xe57d,0x19a2,0x2328,0xdc68,0x116e,0xee21,0x0151,0xff90,0x0025,0x0591,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x6000,0xe890,0x13a1,0xf6c6,0x08ca,0x0e5b,0xf135,0xffb5,0x0025,0x0000,0x0000,0xec14,0x13ec,0x1644,0xe9e2,0x093a,0xf54f,0xffb5,0xff45,0x0000,0x093a,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x31
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 2c
+ {
+ {
+ {0x73,0x69,0x74,0x20,0x64,0x6f,0x77,0x6e,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x04,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x02,
+ 0x00,
+ 0x0c,
+ 0x00,
+ 0x20,
+ 0xf1,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0x04,
+ {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xe7fa,0x14a7,0xf6a0,0x0a66,0x0ea6,0xf135,0xffdb,0xff6a,0x0070,0xffdb,0xe57d,0x19a2,0x2328,0xdc68,0x116e,0xee21,0x0151,0xff90,0x0025,0x0591,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x24
+ },
+ {
+ {0x6000,0xe890,0x13a1,0xf6c6,0x08ca,0x0e5b,0xf135,0xffb5,0x0025,0x0000,0x0000,0xec14,0x13ec,0x1644,0xe9e2,0x093a,0xf54f,0xffb5,0xff45,0x0000,0x093a,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x24
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+ // page 2d
+ {
+ {
+ {0x73,0x69,0x74,0x20,0x64,0x6f,0x77,0x6e,0x00,0x00,0x00,0x00,0x00,0x00},
+ 0x00,
+ 0x14,
+ 0x0a,
+ {0x00,0x00,0x00},
+ 0x02,
+ 0x00,
+ 0x0c,
+ 0x00,
+ 0x20,
+ 0x00,
+ 0x00,
+ {0x00,0x00,0x00,0x00},
+ 0xfd,
+ {0x55,0x55,0x55,0x77,0x77,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},
+ 0x00
+ },
+ {
+ {
+ {0x6000,0xe7fa,0x14a7,0xf6a0,0x0a66,0x0ea6,0xf135,0xffdb,0xff6a,0x0070,0xffdb,0xe57d,0x19a2,0x2328,0xdc68,0x116e,0xee21,0x0151,0xff90,0x0025,0x0591,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x18
+ },
+ {
+ {0x6000,0xe890,0x13a1,0xf6c6,0x08ca,0x0e5b,0xf135,0xffb5,0x0025,0x0000,0x0000,0xec14,0x13ec,0x1644,0xe9e2,0x093a,0xf54f,0xffb5,0xff45,0x0000,0x093a,0x6000,0x6000,0x6000,0x6000,0x6000,0xb500,0xb500,0xb500,0xb500,0xb500},
+ 0x00,
+ 0x18
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ },
+ {
+ {0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000,0x0000},
+ 0x00,
+ 0x00
+ }
+ }
+ },
+};
diff --git a/src/point.c b/src/point.c
deleted file mode 100755
index 8494297759d68ee8a320162090edad22c6165de1..0000000000000000000000000000000000000000
--- a/src/point.c
+++ /dev/null
@@ -1,24 +0,0 @@
-#include <math.h>
-#include "point.h"
-
-// public functions
-void point3d_init(TPoint3D *point)
-{
- point->x=0.0;
- point->y=0.0;
- point->z=0.0;
-}
-
-void point3d_load(TPoint3D *point, float x, float y,float z)
-{
- point->x=x;
- point->y=y;
- point->z=z;
-}
-
-void point3d_copy(TPoint3D *point_dst, TPoint3D *point_src)
-{
- point_dst->x=point_src->x;
- point_dst->y=point_src->y;
- point_dst->z=point_src->z;
-}
diff --git a/src/ram.c b/src/ram.c
index 0942fac54e9aa508f9d687b344a99879c2d13a65..0bdd49976e589d126604cae8c42e207f25854b58 100755
--- a/src/ram.c
+++ b/src/ram.c
@@ -1,11 +1,30 @@
#include "ram.h"
-// private variables
-uint8_t ram_data[RAM_SIZE]={0x00};
+uint8_t ram_data[RAM_SIZE];
-// public functions
void ram_init(void)
{
+ uint16_t eeprom_data;
+
+ // read contents from EEPROM to RAM
+ if(EE_ReadVariable(DEVICE_MODEL_OFFSET,&eeprom_data)==0)
+ ram_data[DEVICE_MODEL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(DEVICE_MODEL_OFFSET+1,&eeprom_data)==0)
+ ram_data[DEVICE_MODEL_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(FIRMWARE_VERSION_OFFSET,&eeprom_data)==0)
+ ram_data[FIRMWARE_VERSION_OFFSET]=(uint8_t)eeprom_data;
+ if(EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data)==0)
+ ram_data[DEVICE_ID_OFFSET]=(uint8_t)eeprom_data;
+ if(EE_ReadVariable(BAUDRATE_OFFSET,&eeprom_data)==0)
+ ram_data[BAUDRATE_OFFSET]=(uint8_t)eeprom_data;
+ if(EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data)==0)
+ ram_data[RETURN_DELAY_OFFSET]=(uint8_t)eeprom_data;
+ if(EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data)==0)
+ ram_data[MM_PERIOD_OFFSET]=(uint8_t)eeprom_data;
+ if(EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data)==0)
+ ram_data[MM_PERIOD_OFFSET+1]=(uint8_t)eeprom_data;
+ if(EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data)==0)
+ ram_data[RETURN_LEVEL_OFFSET]=(uint8_t)eeprom_data;
}
inline void ram_read_byte(uint8_t address,uint8_t *data)
@@ -82,9 +101,18 @@ uint8_t ram_write_table(uint8_t address, uint8_t length,uint8_t *data)
if((address+length)<RAM_SIZE)
{
for(i=0;i<length;i++)
- data[i]=ram_data[address+i];
+ ram_data[address+i]=data[i];
return RAM_SUCCESS;
}
else
return RAM_BAD_ADDRESS;
}
+
+inline uint8_t ram_in_range(uint8_t start_reg,uint8_t end_reg,uint8_t address,uint8_t length)
+{
+ if(start_reg>=address && end_reg<(address+length))
+ return 0x01;
+ else
+ return 0x00;
+}
+
diff --git a/src/spi.c b/src/spi.c
deleted file mode 100755
index 821d6fe5dd1645de82be2f1dd6f72fd0c264ecd1..0000000000000000000000000000000000000000
--- a/src/spi.c
+++ /dev/null
@@ -1,152 +0,0 @@
-#include "spi.h"
-
-#define PIN_SIG_SCK GPIO_Pin_13
-#define PIN_SIG_MISO GPIO_Pin_14
-#define PIN_SIG_MOSI GPIO_Pin_15
-#define PORT_SPI GPIOB
-
-// private variables
-
-// public functions
-void spi_init(void)
-{
- GPIO_InitTypeDef GPIO_InitStructure;
- SPI_InitTypeDef SPI_InitStructure;
-
- // configure the GPIO
- GPIO_StructInit(&GPIO_InitStructure);
- GPIO_InitStructure.GPIO_Pin = PIN_SIG_SCK | PIN_SIG_MOSI | PIN_SIG_MISO;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
- GPIO_Init(PORT_SPI, &GPIO_InitStructure);
-
- // configure the SPI module
- SPI_StructInit(&SPI_InitStructure);
- SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
- SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
- SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
- SPI_InitStructure.SPI_CPOL = SPI_CPOL_High;
- SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;
- SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
- SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_4;
- SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
- SPI_InitStructure.SPI_CRCPolynomial = 7;
- SPI_Init(SPI2, &SPI_InitStructure);
-
- /* Enable SPI2 */
- SPI_Cmd(SPI2, ENABLE);
-}
-
-void spi_read_byte(uint8_t address, uint8_t *data)
-{
- // wait for the SPI Tx buffer to be empty
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
- // send address
- SPI_I2S_SendData(SPI2,address|0x80);
- // wait for the end of transmission
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
- // wait for data to be received
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_RXNE) == RESET);
- // read and discard the received data
- SPI_I2S_ReceiveData(SPI2);
-
- // receive data
- SPI_I2S_SendData(SPI2,0xFF);
- // wait for the end of transmission
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
- // wait for data to be received
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_RXNE) == RESET);
- // read and discard the received data
- *data=SPI_I2S_ReceiveData(SPI2);
-}
-
-void spi_read_word(uint8_t address, uint16_t *data)
-{
- uint8_t value;
-
- // wait for the SPI Tx buffer to be empty
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
- // send address
- SPI_I2S_SendData(SPI2,address|0xC0);//autoincrement
- // wait for the end of transmission
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
- // wait for data to be received
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_RXNE) == RESET);
- // read and discard the received data
- SPI_I2S_ReceiveData(SPI2);
-
- // receive data MSB
- SPI_I2S_SendData(SPI2,0xFF);
- // wait for the end of transmission
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
- // wait for data to be received
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_RXNE) == RESET);
- // read and discard the received data
- value=SPI_I2S_ReceiveData(SPI2);
- (*data)=value;
-
- // receive data LSB
- SPI_I2S_SendData(SPI2,0xFF);
- // wait for the end of transmission
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
- // wait for data to be received
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_RXNE) == RESET);
- // read and discard the received data
- value=SPI_I2S_ReceiveData(SPI2);
- (*data)+=value*256;
-}
-
-void spi_write_byte(uint8_t address, uint8_t data)
-{
- // wait for the SPI Tx buffer to be empty
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
- // send address
- SPI_I2S_SendData(SPI2,address);
- // wait for the end of transmission
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
- // wait for data to be received
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_RXNE) == RESET);
- // read and discard the received data
- SPI_I2S_ReceiveData(SPI2);
-
- // send data
- SPI_I2S_SendData(SPI2,data);
- // wait for the end of transmission
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
- // wait for data to be received
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_RXNE) == RESET);
- // read and discard the received data
- SPI_I2S_ReceiveData(SPI2);
-}
-
-void spi_write_word(uint8_t address, uint16_t data)
-{
- // wait for the SPI Tx buffer to be empty
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
- // send address
- SPI_I2S_SendData(SPI2,address);
- // wait for the end of transmission
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
- // wait for data to be received
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_RXNE) == RESET);
- // read and discard the received data
- SPI_I2S_ReceiveData(SPI2);
-
- // send data MSB
- SPI_I2S_SendData(SPI2,data/256);
- // wait for the end of transmission
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
- // wait for data to be received
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_RXNE) == RESET);
- // read and discard the received data
- SPI_I2S_ReceiveData(SPI2);
-
- // send data LSB
- SPI_I2S_SendData(SPI2,data%256);
- // wait for the end of transmission
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
- // wait for data to be received
- while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_RXNE) == RESET);
- // read and discard the received data
- SPI_I2S_ReceiveData(SPI2);
-}
diff --git a/src/stm32f10x_it.c b/src/stm32f10x_it.c
deleted file mode 100755
index a5722d26a90bc2deb5741d71bd20d2bbb6cd9e34..0000000000000000000000000000000000000000
--- a/src/stm32f10x_it.c
+++ /dev/null
@@ -1,138 +0,0 @@
-/**
- ******************************************************************************
- * @file Project/STM32F10x_StdPeriph_Template/stm32f10x_it.c
- * @author MCD Application Team
- * @version V3.5.0
- * @date 08-April-2011
- * @brief Main Interrupt Service Routines.
- * This file provides template for all exceptions handler and
- * peripherals interrupt service routine.
- ******************************************************************************
- * @attention
- *
- * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
- * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
- * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
- * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
- * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
- * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
- *
- * <h2><center>© COPYRIGHT 2011 STMicroelectronics</center></h2>
- ******************************************************************************
- */
-
-/* Includes ------------------------------------------------------------------*/
-#include "stm32f10x_it.h"
-#include "gpio.h"
-
-/**
- * @brief This function handles NMI exception.
- * @param None
- * @retval None
- */
-void NMI_Handler(void)
-{
- /* This interrupt is generated when HSE clock fails */
-
- if (RCC_GetITStatus(RCC_IT_CSS) != RESET)
- {/* At this stage: HSE, PLL are disabled (but no change on PLL config) and HSI
- is selected as system clock source */
-
- /* Enable HSE */
- RCC_HSEConfig(RCC_HSE_ON);
-
- /* Enable HSE Ready interrupt */
- RCC_ITConfig(RCC_IT_HSERDY, ENABLE);
-
- /* Enable PLL Ready interrupt */
- RCC_ITConfig(RCC_IT_PLLRDY, ENABLE);
-
- /* Clear Clock Security System interrupt pending bit */
- RCC_ClearITPendingBit(RCC_IT_CSS);
-
- /* Once HSE clock recover, the HSERDY interrupt is generated and in the RCC ISR
- routine the system clock will be reconfigured to its previous state (before
- HSE clock failure) */
- }
-}
-
-/**
- * @brief This function handles Hard Fault exception.
- * @param None
- * @retval None
- */
-void HardFault_Handler(void)
-{
- /* Go to infinite loop when Hard Fault exception occurs */
- while (1)
- {
- }
-}
-
-/**
- * @brief This function handles Memory Manage exception.
- * @param None
- * @retval None
- */
-void MemManage_Handler(void)
-{
- /* Go to infinite loop when Memory Manage exception occurs */
- while (1)
- {
- }
-}
-
-/**
- * @brief This function handles Bus Fault exception.
- * @param None
- * @retval None
- */
-void BusFault_Handler(void)
-{
- /* Go to infinite loop when Bus Fault exception occurs */
- while (1)
- {
- }
-}
-
-/**
- * @brief This function handles Usage Fault exception.
- * @param None
- * @retval None
- */
-void UsageFault_Handler(void)
-{
- /* Go to infinite loop when Usage Fault exception occurs */
- while (1)
- {
- }
-}
-
-/**
- * @brief This function handles SVCall exception.
- * @param None
- * @retval None
- */
-void SVC_Handler(void)
-{
-}
-
-/**
- * @brief This function handles Debug Monitor exception.
- * @param None
- * @retval None
- */
-void DebugMon_Handler(void)
-{
-}
-
-/**
- * @brief This function handles PendSV_Handler exception.
- * @param None
- * @retval None
- */
-void PendSV_Handler(void)
-{
-}
-
-
diff --git a/src/system_init.c b/src/system_init.c
deleted file mode 100755
index 0d0da3e95769359695da1d9d5266c704d2a80e75..0000000000000000000000000000000000000000
--- a/src/system_init.c
+++ /dev/null
@@ -1,124 +0,0 @@
-#include "system_init.h"
-
-// interrupt handlers
-/**
- * @brief This function handles RCC interrupt request.
- * @param None
- * @retval None
- */
-void RCC_IRQHandler(void)
-{
- if(RCC_GetITStatus(RCC_IT_HSERDY) != RESET)
- {
- /* Clear HSERDY interrupt pending bit */
- RCC_ClearITPendingBit(RCC_IT_HSERDY);
-
- /* Check if the HSE clock is still available */
- if (RCC_GetFlagStatus(RCC_FLAG_HSERDY) != RESET)
- {
- /* Enable PLL: once the PLL is ready the PLLRDY interrupt is generated */
- RCC_PLLCmd(ENABLE);
- }
- }
-
- if(RCC_GetITStatus(RCC_IT_PLLRDY) != RESET)
- {
- /* Clear PLLRDY interrupt pending bit */
- RCC_ClearITPendingBit(RCC_IT_PLLRDY);
-
- /* Check if the PLL is still locked */
- if (RCC_GetFlagStatus(RCC_FLAG_PLLRDY) != RESET)
- {
- /* Select PLL as system clock source */
- RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);
- }
- }
-}
-
-// public functions
-void clocks_init(void)
-{
- ErrorStatus HSEStartUpStatus;
- NVIC_InitTypeDef NVIC_InitStructure;
-
- /* SYSCLK, HCLK, PCLK2 and PCLK1 configuration -----------------------------*/
- /* RCC system reset(for debug purpose) */
- RCC_DeInit();
-
- /* Enable HSE */
- RCC_HSEConfig(RCC_HSE_ON);
-
- /* Wait till HSE is ready */
- HSEStartUpStatus = RCC_WaitForHSEStartUp();
-
- if (HSEStartUpStatus == SUCCESS)
- {
- /* Enable Prefetch Buffer */
- FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
-
- /* Flash 2 wait state */
- FLASH_SetLatency(FLASH_Latency_2);
-
- /* HCLK = SYSCLK */
- RCC_HCLKConfig(RCC_SYSCLK_Div1);
-
- /* PCLK2 = HCLK */
- RCC_PCLK2Config(RCC_HCLK_Div1);
-
- /* PCLK1 = HCLK/2 */
- RCC_PCLK1Config(RCC_HCLK_Div2);
-
- /* PLLCLK = 8MHz * 9 = 72 MHz */
- RCC_PLLConfig(RCC_PLLSource_HSE_Div1, RCC_PLLMul_9);
-
- /* Enable PLL */
- RCC_PLLCmd(ENABLE);
-
- /* Wait till PLL is ready */
- while (RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET)
- {
- }
-
- /* Select PLL as system clock source */
- RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);
-
- /* Wait till PLL is used as system clock source */
- while(RCC_GetSYSCLKSource() != 0x08)
- {
- }
-
- /* Enable Clock Security System(CSS): this will generate an NMI exception
- when HSE clock fails */
- RCC_ClockSecuritySystemCmd(ENABLE);
-
- // place the vector table into FLASH
- NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x3000);
-
- /* Configure the NVIC Preemption Priority Bits */
- NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
-
- /* Enable and configure RCC global IRQ channel */
- NVIC_InitStructure.NVIC_IRQChannel = RCC_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
- /* enable the peripheral clocks */
- /* DMA clock enable */
- /* enable GPIO's, USART1, ADC */
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1 | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOB |
- RCC_APB2Periph_AFIO | RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE);
-
- /* enable USART3 */
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2 | RCC_APB1Periph_USART3 | RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 ,ENABLE);
- }
- else
- { /* If HSE fails to start-up, the application will have wrong clock configuration.
- User can add here some code to deal with this error */
-
- /* Go to infinite loop */
- while (1)
- {
- }
- }
-}
diff --git a/src/system_stm32f10x.c b/src/system_stm32f10x.c
index 6fb4579ec804e799132b5587a10d980a573916bb..315aac734d3c6c55ca81573907a31e20c086fef0 100755
--- a/src/system_stm32f10x.c
+++ b/src/system_stm32f10x.c
@@ -125,7 +125,7 @@
/*!< Uncomment the following line if you need to relocate your vector Table in
Internal SRAM. */
/* #define VECT_TAB_SRAM */
-#define VECT_TAB_OFFSET 0x0 /*!< Vector Table base offset field.
+#define VECT_TAB_OFFSET 0x3000 /*!< Vector Table base offset field.
This value must be a multiple of 0x200. */
@@ -266,6 +266,9 @@ void SystemInit (void)
#else
SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH. */
#endif
+
+ /* Configure the NVIC Preemption Priority Bits */
+ NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
}
/**
diff --git a/src/time.c b/src/time.c
index 343f2cc205d2b34e7642a26e3026860be5b27a70..e36a0d2faf74a0998930771b2746028a49c1e8df 100755
--- a/src/time.c
+++ b/src/time.c
@@ -20,6 +20,7 @@ void time_init(void)
{
// set the time base to 1ms
SysTick_Config(SystemCoreClock / 1000);
+ NVIC_SetPriority(SysTick_IRQn,0);
}
void delay_ms(__IO uint32_t time)
diff --git a/src/utils.c b/src/utils.c
deleted file mode 100755
index aced8ee2bed55f3858255a499395966f19977008..0000000000000000000000000000000000000000
--- a/src/utils.c
+++ /dev/null
@@ -1,17 +0,0 @@
-#include "utils.h"
-
-uint16_t make_word(uint8_t msb,uint8_t lsb)
-{
- return ((msb<<8)+lsb);
-}
-
-inline uint8_t get_msb(uint16_t data)
-{
- return ((data&0xFF00)>>8);
-}
-
-inline uint8_t get_lsb(uint16_t data)
-{
- return (data&0x00FF);
-}
-
diff --git a/src/vector.c b/src/vector.c
deleted file mode 100755
index 322cf6111e0a568583afed0282cde2b02415cf9b..0000000000000000000000000000000000000000
--- a/src/vector.c
+++ /dev/null
@@ -1,29 +0,0 @@
-#include <math.h>
-#include "vector.h"
-
-// public functions
-void vector3d_init(TVector3D *vector)
-{
- vector->x=0.0;
- vector->y=0.0;
- vector->z=0.0;
-}
-
-void vector3d_load(TVector3D *vector, float x, float y, float z)
-{
- vector->x=x;
- vector->y=y;
- vector->z=z;
-}
-
-void vector3d_copy(TVector3D *vector_dst, TVector3D *vector_src)
-{
- vector_dst->x=vector_src->x;
- vector_dst->y=vector_src->y;
- vector_dst->z=vector_src->z;
-}
-
-float vector3d_length(TVector3D *vector)
-{
- return sqrt(vector->x*vector->x + vector->y*vector->y + vector->z*vector->z);
-}
diff --git a/src/walking.c b/src/walking.c
index 6580c6d663434111da2b31236a9ebcec6d7216c1..2c3a264235dafb8fba2a6ea14042479a37838248 100755
--- a/src/walking.c
+++ b/src/walking.c
@@ -1,45 +1,33 @@
#include "walking.h"
#include "motion_manager.h"
-#include "point.h"
-#include "vector.h"
-#include "matrix.h"
#include "ram.h"
-#include <math.h>
+#include "darwin_kinematics.h"
+#include "darwin_math.h"
-// private constants
-#define PI 3.14159
+enum {PHASE0 = 0,PHASE1 = 1,PHASE2 = 2, PHASE3 = 3};
-enum
-{
- PHASE0 = 0,
- PHASE1 = 1,
- PHASE2 = 2,
- PHASE3 = 3
-};
+int64_t walking_current_angles[MANAGER_MAX_NUM_SERVOS];
// leg kinematics
-const float LEG_SIDE_OFFSET = 37.0; //mm
-const float THIGH_LENGTH = 93.0; //mm
-const float CALF_LENGTH = 93.0; //mm
-const float ANKLE_LENGTH = 33.5; //mm
-const float LEG_LENGTH = 219.5; //mm (THIGH_LENGTH + CALF_LENGTH + ANKLE_LENGTH)
const uint8_t servo_seq[14]={R_HIP_YAW,R_HIP_ROLL,R_HIP_PITCH,R_KNEE,R_ANKLE_PITCH,R_ANKLE_ROLL,L_HIP_YAW,L_HIP_ROLL,L_HIP_PITCH,L_KNEE,L_ANKLE_PITCH,L_ANKLE_ROLL,R_SHOULDER_PITCH,L_SHOULDER_PITCH};
+const int8_t dir[14]={-1,-1,1,1,-1,1,-1,-1,-1,-1,1,1,1,-1};
+
// walking configuration variables
-float Y_SWAP_AMPLITUDE=20.0;
-float Z_SWAP_AMPLITUDE=5;
+float Y_SWAP_AMPLITUDE=20.0;//mm
+float Z_SWAP_AMPLITUDE=5;//mm
float ARM_SWING_GAIN=1.5;
-float PELVIS_OFFSET=3.0;
-float HIP_PITCH_OFFSET=23.0;
+float PELVIS_OFFSET=3.0;//degrees
+float HIP_PITCH_OFFSET=23.0;//degrees
uint8_t P_GAIN=32;
uint8_t I_GAIN=0;
uint8_t D_GAIN=0;
// Walking initial pose
-float X_OFFSET=-10;
-float Y_OFFSET=5;
-float Z_OFFSET=20;
+float X_OFFSET=-10;//mm
+float Y_OFFSET=5;//mm
+float Z_OFFSET=20;//mm
float A_OFFSET=0;
float P_OFFSET=0;
float R_OFFSET=0;
@@ -48,10 +36,10 @@ float R_OFFSET=0;
float PERIOD_TIME=600;
float DSP_RATIO=0.1;
float STEP_FB_RATIO=0.3;
-float X_MOVE_AMPLITUDE=0;
-float Y_MOVE_AMPLITUDE=0;
-float Z_MOVE_AMPLITUDE=40;
-float A_MOVE_AMPLITUDE=0;
+float X_MOVE_AMPLITUDE=10;//mm
+float Y_MOVE_AMPLITUDE=0;//mm
+float Z_MOVE_AMPLITUDE=40;//mm
+float A_MOVE_AMPLITUDE=0;//degrees
uint8_t A_MOVE_AIM_ON=0x00;
// private variables
@@ -116,90 +104,11 @@ float m_Body_Swing_Y;
float m_Body_Swing_Z;
// private functions
-float wsin(float time, float period, float period_shift, float mag, float mag_shift)
+inline float wsin(float time, float period, float period_shift, float mag, float mag_shift)
{
return mag * sin(2 * 3.141592 / period * time - period_shift) + mag_shift;
}
-uint8_t computeIK(float *out, float x, float y, float z, float a, float b, float c)
-{
- TMatrix3D Tad, Tda, Tcd, Tdc, Tac;
- TVector3D vec,orientation;
- TPoint3D position;
- float _Rac, _Acos, _Atan, _k, _l, _m, _n, _s, _c, _theta;
-
- vector3d_load(&orientation,a * 180.0 / PI, b * 180.0 / PI, c * 180.0 / PI);
- point3d_load(&position,x, y, z - LEG_LENGTH);
- matrix3d_set_transform(&Tad,&position,&orientation);
-
- vec.x = x + Tad.coef[2] * ANKLE_LENGTH;
- vec.y = y + Tad.coef[6] * ANKLE_LENGTH;
- vec.z = (z - LEG_LENGTH) + Tad.coef[10] * ANKLE_LENGTH;
-
- // Get Knee
- _Rac = vector3d_length(&vec);
- _Acos = acos((_Rac * _Rac - THIGH_LENGTH * THIGH_LENGTH - CALF_LENGTH * CALF_LENGTH) / (2 * THIGH_LENGTH * CALF_LENGTH));
- if(isnan(_Acos) == 1)
- return 0x00;;
- *(out + 3) = _Acos;
-
- // Get Ankle Roll
- Tda = Tad;
- if(matrix3d_inverse(&Tda,&Tda) == 0x00)
- return 0x00;
- _k = sqrt(Tda.coef[7] * Tda.coef[7] + Tda.coef[11] * Tda.coef[11]);
- _l = sqrt(Tda.coef[7] * Tda.coef[7] + (Tda.coef[11] - ANKLE_LENGTH) * (Tda.coef[11] - ANKLE_LENGTH));
- _m = (_k * _k - _l * _l - ANKLE_LENGTH * ANKLE_LENGTH) / (2 * _l * ANKLE_LENGTH);
- if(_m > 1.0)
- _m = 1.0;
- else if(_m < -1.0)
- _m = -1.0;
- _Acos = acos(_m);
- if(isnan(_Acos) == 1)
- return 0x00;
- if(Tda.coef[7] < 0.0)
- *(out + 5) = -_Acos;
- else
- *(out + 5) = _Acos;
- // Get Hip Yaw
- vector3d_load(&orientation,*(out + 5) * 180.0 / PI, 0, 0);
- point3d_load(&position,0, 0, -ANKLE_LENGTH);
- matrix3d_set_transform(&Tcd,&position,&orientation);
- Tdc = Tcd;
- if(matrix3d_inverse(&Tdc,&Tdc) == 0x00)
- return 0x00;
- matrix3d_mult(&Tac,&Tad,&Tdc);
- _Atan = atan2(-Tac.coef[1] , Tac.coef[5]);
- if(isinf(_Atan) == 1)
- return 0x00;
- *(out) = _Atan;
-
- // Get Hip Roll
- _Atan = atan2(Tac.coef[9], -Tac.coef[1] * sin(*(out)) + Tac.coef[5] * cos(*(out)));
- if(isinf(_Atan) == 1)
- return 0x00;
- *(out + 1) = _Atan;
-
- // Get Hip Pitch and Ankle Pitch
- _Atan = atan2(Tac.coef[2] * cos(*(out)) + Tac.coef[6] * sin(*(out)), Tac.coef[0] * cos(*(out)) + Tac.coef[4] * sin(*(out)));
- if(isinf(_Atan) == 1)
- return 0x00;
- _theta = _Atan;
- _k = sin(*(out + 3)) * CALF_LENGTH;
- _l = -THIGH_LENGTH - cos(*(out + 3)) * CALF_LENGTH;
- _m = cos(*(out)) * vec.x + sin(*(out)) * vec.y;
- _n = cos(*(out + 1)) * vec.z + sin(*(out)) * sin(*(out + 1)) * vec.x - cos(*(out)) * sin(*(out + 1)) * vec.y;
- _s = (_k * _n + _l * _m) / (_k * _k + _l * _l);
- _c = (_n - _k * _s) / _l;
- _Atan = atan2(_s, _c);
- if(isinf(_Atan) == 1)
- return 0x00;
- *(out + 2) = _Atan;
- *(out + 4) = _theta - *(out + 3) - *(out + 2);
-
- return 0x01;
-}
-
void update_param_time()
{
m_PeriodTime = PERIOD_TIME;
@@ -283,27 +192,27 @@ void update_param_balance()
void walking_init(void)
{
// initialize RAM
- ram_write_byte(CM730_X_OFFSET,(uint8_t)X_OFFSET);
- ram_write_byte(CM730_Y_OFFSET,(uint8_t)Y_OFFSET);
- ram_write_byte(CM730_Z_OFFSET,(uint8_t)Z_OFFSET);
- ram_write_byte(CM730_ROLL,(uint8_t)(A_OFFSET*10));
- ram_write_byte(CM730_PITCH,(uint8_t)(P_OFFSET*10));
- ram_write_byte(CM730_YAW,(uint8_t)(R_OFFSET*10));
- ram_write_word(CM730_HIP_PITCH_OFF_L,(uint16_t)(HIP_PITCH_OFFSET*10));
- ram_write_word(CM730_PERIOD_TIME_L,(uint16_t)PERIOD_TIME);
- ram_write_byte(CM730_DSP_RATIO,(uint8_t)(DSP_RATIO*10));
- ram_write_byte(CM730_STEP_FW_BW_RATIO,(uint8_t)(STEP_FB_RATIO*100));
- ram_write_byte(CM730_STEP_FW_BW,(uint8_t)(X_MOVE_AMPLITUDE));
- ram_write_byte(CM730_STEP_LEFT_RIGHT,(uint8_t)(Y_MOVE_AMPLITUDE));
- ram_write_byte(CM730_STEP_DIRECTION,(uint8_t)(A_MOVE_AMPLITUDE));
- ram_write_byte(CM730_FOOT_HEIGHT,(uint8_t)(Z_MOVE_AMPLITUDE));
- ram_write_byte(CM730_SWING_RIGHT_LEFT,(uint8_t)(Y_SWAP_AMPLITUDE*10));
- ram_write_byte(CM730_SWING_TOP_DOWN,(uint8_t)(Z_SWAP_AMPLITUDE*10));
- ram_write_byte(CM730_PELVIS_OFFSET,(uint8_t)(PELVIS_OFFSET));
- ram_write_byte(CM730_ARM_SWING_GAIN,(uint8_t)(ARM_SWING_GAIN*10));
- ram_write_byte(CM730_P_GAIN,(uint8_t)(P_GAIN));
- ram_write_byte(CM730_I_GAIN,(uint8_t)(I_GAIN));
- ram_write_byte(CM730_D_GAIN,(uint8_t)(D_GAIN));
+ ram_write_byte(DARWIN_X_OFFSET,(uint8_t)X_OFFSET);
+ ram_write_byte(DARWIN_Y_OFFSET,(uint8_t)Y_OFFSET);
+ ram_write_byte(DARWIN_Z_OFFSET,(uint8_t)Z_OFFSET);
+ ram_write_byte(DARWIN_ROLL,(uint8_t)(A_OFFSET*10));
+ ram_write_byte(DARWIN_PITCH,(uint8_t)(P_OFFSET*10));
+ ram_write_byte(DARWIN_YAW,(uint8_t)(R_OFFSET*10));
+ ram_write_word(DARWIN_HIP_PITCH_OFF_L,(uint16_t)(HIP_PITCH_OFFSET*10));
+ ram_write_word(DARWIN_PERIOD_TIME_L,(uint16_t)PERIOD_TIME);
+ ram_write_byte(DARWIN_DSP_RATIO,(uint8_t)(DSP_RATIO*10));
+ ram_write_byte(DARWIN_STEP_FW_BW_RATIO,(uint8_t)(STEP_FB_RATIO*100));
+ ram_write_byte(DARWIN_STEP_FW_BW,(uint8_t)(X_MOVE_AMPLITUDE));
+ ram_write_byte(DARWIN_STEP_LEFT_RIGHT,(uint8_t)(Y_MOVE_AMPLITUDE));
+ ram_write_byte(DARWIN_STEP_DIRECTION,(uint8_t)(A_MOVE_AMPLITUDE));
+ ram_write_byte(DARWIN_FOOT_HEIGHT,(uint8_t)(Z_MOVE_AMPLITUDE));
+ ram_write_byte(DARWIN_SWING_RIGHT_LEFT,(uint8_t)(Y_SWAP_AMPLITUDE*10));
+ ram_write_byte(DARWIN_SWING_TOP_DOWN,(uint8_t)(Z_SWAP_AMPLITUDE*10));
+ ram_write_byte(DARWIN_PELVIS_OFFSET,(uint8_t)(PELVIS_OFFSET));
+ ram_write_byte(DARWIN_ARM_SWING_GAIN,(uint8_t)(ARM_SWING_GAIN*10));
+ ram_write_byte(DARWIN_P_GAIN,(uint8_t)(P_GAIN));
+ ram_write_byte(DARWIN_I_GAIN,(uint8_t)(I_GAIN));
+ ram_write_byte(DARWIN_D_GAIN,(uint8_t)(D_GAIN));
// initialize the internal variables
m_X_Swap_Phase_Shift=PI;
m_X_Swap_Amplitude_Shift=0.0;
@@ -367,16 +276,16 @@ uint8_t walking_is_aim_enabled(void)
void walking_start(void)
{
- ram_write_byte(CM730_WALKING_STATUS,0x01);
- ram_set_bit(CM730_WALKING_CONTROL,0x00);
- ram_clear_bit(CM730_WALKING_CONTROL,0x01);
+ ram_write_byte(DARWIN_WALK_STATUS,0x01);
+ ram_set_bit(DARWIN_WALK_CONTROL,0x00);
+ ram_clear_bit(DARWIN_WALK_CONTROL,0x01);
m_Ctrl_Running=0x01;
}
void walking_stop(void)
{
- ram_set_bit(CM730_WALKING_CONTROL,0x01);
- ram_clear_bit(CM730_WALKING_CONTROL,0x00);
+ ram_set_bit(DARWIN_WALK_CONTROL,0x01);
+ ram_clear_bit(DARWIN_WALK_CONTROL,0x00);
m_Ctrl_Running=0x00;
}
@@ -384,7 +293,7 @@ uint8_t is_walking(void)
{
uint8_t walking;
- ram_read_byte(CM730_WALKING_STATUS,&walking);
+ ram_read_byte(DARWIN_WALK_STATUS,&walking);
return walking;
}
@@ -399,8 +308,8 @@ void walking_process(void)
float angle[14], ep[12];
uint8_t walking,i;
- // R_HIP_YAW, R_HIP_ROLL, R_HIP_PITCH, R_KNEE, R_ANKLE_PITCH, R_ANKLE_ROLL, L_HIP_YAW, L_HIP_ROLL, L_HIP_PITCH, L_KNEE, L_ANKLE_PITCH, L_ANKLE_ROLL, R_ARM_SWING, L_ARM_SWING
- float initAngle[14] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -483.45, 413.13 };
+ //R_HIP_YAW, R_HIP_ROLL, R_HIP_PITCH, R_KNEE, R_ANKLE_PITCH, R_ANKLE_ROLL, L_HIP_YAW, L_HIP_ROLL, L_HIP_PITCH, L_KNEE, L_ANKLE_PITCH, L_ANKLE_ROLL, R_ARM_SWING, L_ARM_SWING
+ float initAngle[14] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-48.345,41.313};
if(m_Time == 0)
{
@@ -409,7 +318,7 @@ void walking_process(void)
if(m_Ctrl_Running == 0x00)
{
if(m_X_Move_Amplitude == 0 && m_Y_Move_Amplitude == 0 && m_A_Move_Amplitude == 0)
- ram_write_byte(CM730_WALKING_STATUS,0x00);
+ ram_write_byte(DARWIN_WALK_STATUS,0x00);
else
{
X_MOVE_AMPLITUDE = 0;
@@ -431,7 +340,7 @@ void walking_process(void)
if(m_Ctrl_Running == 0x00)
{
if(m_X_Move_Amplitude == 0 && m_Y_Move_Amplitude == 0 && m_A_Move_Amplitude == 0)
- ram_write_byte(CM730_WALKING_STATUS,0x00);
+ ram_write_byte(DARWIN_WALK_STATUS,0x00);
else
{
X_MOVE_AMPLITUDE = 0;
@@ -456,66 +365,66 @@ void walking_process(void)
c_swap = 0;
if(m_Time <= m_SSP_Time_Start_L)
{
- x_move_l = wsin(m_SSP_Time_Start_L, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2.0 * PI / m_X_Move_PeriodTime * m_SSP_Time_Start_L, m_X_Move_Amplitude, m_X_Move_Amplitude_Shift);
- y_move_l = wsin(m_SSP_Time_Start_L, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2.0 * PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_L, m_Y_Move_Amplitude, m_Y_Move_Amplitude_Shift);
- z_move_l = wsin(m_SSP_Time_Start_L, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2.0 * PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_L, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
- c_move_l = wsin(m_SSP_Time_Start_L, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2.0 * PI / m_A_Move_PeriodTime * m_SSP_Time_Start_L, m_A_Move_Amplitude, m_A_Move_Amplitude_Shift);
- x_move_r = wsin(m_SSP_Time_Start_L, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2.0 * PI / m_X_Move_PeriodTime * m_SSP_Time_Start_L, -m_X_Move_Amplitude, -m_X_Move_Amplitude_Shift);
- y_move_r = wsin(m_SSP_Time_Start_L, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2.0 * PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_L, -m_Y_Move_Amplitude, -m_Y_Move_Amplitude_Shift);
- z_move_r = wsin(m_SSP_Time_Start_R, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2.0 * PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_R, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
- c_move_r = wsin(m_SSP_Time_Start_L, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2.0 * PI / m_A_Move_PeriodTime * m_SSP_Time_Start_L, -m_A_Move_Amplitude, -m_A_Move_Amplitude_Shift);
+ x_move_l=wsin(m_SSP_Time_Start_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
+ y_move_l=wsin(m_SSP_Time_Start_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
+ z_move_l=wsin(m_SSP_Time_Start_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_l=wsin(m_SSP_Time_Start_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
+ x_move_r=wsin(m_SSP_Time_Start_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
+ y_move_r=wsin(m_SSP_Time_Start_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
+ z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_r=wsin(m_SSP_Time_Start_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
pelvis_offset_l = 0;
pelvis_offset_r = 0;
}
else if(m_Time <= m_SSP_Time_End_L)
{
- x_move_l = wsin(m_Time, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2.0 * PI / m_X_Move_PeriodTime * m_SSP_Time_Start_L, m_X_Move_Amplitude, m_X_Move_Amplitude_Shift);
- y_move_l = wsin(m_Time, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2.0 * PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_L, m_Y_Move_Amplitude, m_Y_Move_Amplitude_Shift);
- z_move_l = wsin(m_Time, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2.0 * PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_L, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
- c_move_l = wsin(m_Time, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2.0 * PI / m_A_Move_PeriodTime * m_SSP_Time_Start_L, m_A_Move_Amplitude, m_A_Move_Amplitude_Shift);
- x_move_r = wsin(m_Time, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2.0 * PI / m_X_Move_PeriodTime * m_SSP_Time_Start_L, -m_X_Move_Amplitude, -m_X_Move_Amplitude_Shift);
- y_move_r = wsin(m_Time, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2.0 * PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_L, -m_Y_Move_Amplitude, -m_Y_Move_Amplitude_Shift);
- z_move_r = wsin(m_SSP_Time_Start_R, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2.0 * PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_R, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
- c_move_r = wsin(m_Time, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2.0 * PI / m_A_Move_PeriodTime * m_SSP_Time_Start_L, -m_A_Move_Amplitude, -m_A_Move_Amplitude_Shift);
- pelvis_offset_l = wsin(m_Time, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2.0 * PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_L, m_Pelvis_Swing / 2.0, m_Pelvis_Swing / 2.0);
- pelvis_offset_r = wsin(m_Time, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2.0 * PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_L, -m_Pelvis_Offset / 2.0, -m_Pelvis_Offset / 2.0);
+ x_move_l=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
+ y_move_l=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
+ z_move_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_l=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
+ x_move_r=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
+ y_move_r=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
+ z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_r=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
+ pelvis_offset_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Pelvis_Swing/2.0,m_Pelvis_Swing/2.0);
+ pelvis_offset_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,-m_Pelvis_Offset/2.0,-m_Pelvis_Offset/2.0);
}
else if(m_Time <= m_SSP_Time_Start_R)
{
- x_move_l = wsin(m_SSP_Time_End_L, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2.0 * PI / m_X_Move_PeriodTime * m_SSP_Time_Start_L, m_X_Move_Amplitude, m_X_Move_Amplitude_Shift);
- y_move_l = wsin(m_SSP_Time_End_L, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2.0 * PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_L, m_Y_Move_Amplitude, m_Y_Move_Amplitude_Shift);
- z_move_l = wsin(m_SSP_Time_End_L, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2.0 * PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_L, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
- c_move_l = wsin(m_SSP_Time_End_L, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2.0 * PI / m_A_Move_PeriodTime * m_SSP_Time_Start_L, m_A_Move_Amplitude, m_A_Move_Amplitude_Shift);
- x_move_r = wsin(m_SSP_Time_End_L, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2.0 * PI / m_X_Move_PeriodTime * m_SSP_Time_Start_L, -m_X_Move_Amplitude, -m_X_Move_Amplitude_Shift);
- y_move_r = wsin(m_SSP_Time_End_L, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2.0 * PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_L, -m_Y_Move_Amplitude, -m_Y_Move_Amplitude_Shift);
- z_move_r = wsin(m_SSP_Time_Start_R, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2.0 * PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_R, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
- c_move_r = wsin(m_SSP_Time_End_L, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2.0 * PI / m_A_Move_PeriodTime * m_SSP_Time_Start_L, -m_A_Move_Amplitude, -m_A_Move_Amplitude_Shift);
+ x_move_l=wsin(m_SSP_Time_End_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
+ y_move_l=wsin(m_SSP_Time_End_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
+ z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_l=wsin(m_SSP_Time_End_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
+ x_move_r=wsin(m_SSP_Time_End_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
+ y_move_r=wsin(m_SSP_Time_End_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
+ z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_r=wsin(m_SSP_Time_End_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
pelvis_offset_l = 0;
pelvis_offset_r = 0;
}
else if(m_Time <= m_SSP_Time_End_R)
{
- x_move_l = wsin(m_Time, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2.0 * PI / m_X_Move_PeriodTime * m_SSP_Time_Start_R + PI, m_X_Move_Amplitude, m_X_Move_Amplitude_Shift);
- y_move_l = wsin(m_Time, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2.0 * PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_R + PI, m_Y_Move_Amplitude, m_Y_Move_Amplitude_Shift);
- z_move_l = wsin(m_SSP_Time_End_L, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2.0 * PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_L, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
- c_move_l = wsin(m_Time, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2.0 * PI / m_A_Move_PeriodTime * m_SSP_Time_Start_R + PI, m_A_Move_Amplitude, m_A_Move_Amplitude_Shift);
- x_move_r = wsin(m_Time, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2.0 * PI / m_X_Move_PeriodTime * m_SSP_Time_Start_R + PI, -m_X_Move_Amplitude, -m_X_Move_Amplitude_Shift);
- y_move_r = wsin(m_Time, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2.0 * PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_R + PI, -m_Y_Move_Amplitude, -m_Y_Move_Amplitude_Shift);
- z_move_r = wsin(m_Time, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2.0 * PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_R, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
- c_move_r = wsin(m_Time, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2.0 * PI / m_A_Move_PeriodTime * m_SSP_Time_Start_R + PI, -m_A_Move_Amplitude, -m_A_Move_Amplitude_Shift);
- pelvis_offset_l = wsin(m_Time, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2.0 * PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_R, m_Pelvis_Offset / 2, m_Pelvis_Offset / 2);
- pelvis_offset_r = wsin(m_Time, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2.0 * PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_R, -m_Pelvis_Swing / 2, -m_Pelvis_Swing / 2);
+ x_move_l=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
+ y_move_l=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
+ z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_l=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
+ x_move_r=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
+ y_move_r=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
+ z_move_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_r=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
+ pelvis_offset_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Pelvis_Offset/2,m_Pelvis_Offset/2);
+ pelvis_offset_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,-m_Pelvis_Swing/2,-m_Pelvis_Swing/2);
}
else
{
- x_move_l = wsin(m_SSP_Time_End_R, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2.0 * PI / m_X_Move_PeriodTime * m_SSP_Time_Start_R + PI, m_X_Move_Amplitude, m_X_Move_Amplitude_Shift);
- y_move_l = wsin(m_SSP_Time_End_R, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2.0 * PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_R + PI, m_Y_Move_Amplitude, m_Y_Move_Amplitude_Shift);
- z_move_l = wsin(m_SSP_Time_End_L, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2.0 * PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_L, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
- c_move_l = wsin(m_SSP_Time_End_R, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2.0 * PI / m_A_Move_PeriodTime * m_SSP_Time_Start_R + PI, m_A_Move_Amplitude, m_A_Move_Amplitude_Shift);
- x_move_r = wsin(m_SSP_Time_End_R, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2.0 * PI / m_X_Move_PeriodTime * m_SSP_Time_Start_R + PI, -m_X_Move_Amplitude, -m_X_Move_Amplitude_Shift);
- y_move_r = wsin(m_SSP_Time_End_R, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2.0 * PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_R + PI, -m_Y_Move_Amplitude, -m_Y_Move_Amplitude_Shift);
- z_move_r = wsin(m_SSP_Time_End_R, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2.0 * PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_R, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
- c_move_r = wsin(m_SSP_Time_End_R, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2.0 * PI / m_A_Move_PeriodTime * m_SSP_Time_Start_R + PI, -m_A_Move_Amplitude, -m_A_Move_Amplitude_Shift);
+ x_move_l=wsin(m_SSP_Time_End_R,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
+ y_move_l=wsin(m_SSP_Time_End_R,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
+ z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_l=wsin(m_SSP_Time_End_R,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
+ x_move_r=wsin(m_SSP_Time_End_R,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
+ y_move_r=wsin(m_SSP_Time_End_R,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
+ z_move_r=wsin(m_SSP_Time_End_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
+ c_move_r=wsin(m_SSP_Time_End_R,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
pelvis_offset_l = 0;
pelvis_offset_r = 0;
}
@@ -560,10 +469,10 @@ void walking_process(void)
angle[12] = wsin(m_Time, m_PeriodTime, PI * 1.5, -m_X_Move_Amplitude * m_Arm_Swing_Gain, 0);
angle[13] = wsin(m_Time, m_PeriodTime, PI * 1.5, m_X_Move_Amplitude * m_Arm_Swing_Gain, 0);
}
- ram_read_byte(CM730_WALKING_STATUS,&walking);
+ ram_read_byte(DARWIN_WALK_STATUS,&walking);
if(walking == 0x01)
{
- m_Time += manager_get_period();
+ m_Time += 7.8;//manager_get_period();
if(m_Time >= m_PeriodTime)
m_Time = 0;
}
@@ -580,18 +489,14 @@ void walking_process(void)
// Compute motor value
for(i=0; i<14; i++)
{
- if(manager_get_servo_module(servo_seq[i])==MM_WALKING)
- {
- if(i==1)
- initAngle[i]+=(float)dir[i]*(angle[i] + pelvis_offset_r)*10.0;
- else if(i==7)
- initAngle[i]+=(float)dir[i]*(angle[i] + pelvis_offset_l)*10.0;
- else if(i==2 || i==8)
- initAngle[i]+=(float)dir[i]*(angle[i] - HIP_PITCH_OFFSET)*10.0;
- else
- initAngle[i]+=(float)dir[i] * angle[i]*10.0;
- manager_set_servo_angle(servo_seq[i],initAngle[i]);
- }
+ if(i==1)
+ walking_current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] + pelvis_offset_r))*128;// format 9|7
+ else if(i==7)
+ walking_current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] + pelvis_offset_l))*128;// format 9|7
+ else if(i==2 || i==8)
+ walking_current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] - HIP_PITCH_OFFSET))*128;// format 9|7
+ else
+ walking_current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i] * angle[i])*128;//format 9|7
}
}