From 9fef7baf40ae596c06bbd49716f69b5f01afbf39 Mon Sep 17 00:00:00 2001
From: Irene Garcia Camacho <igarcia@iri.upc.edu>
Date: Thu, 23 Mar 2017 19:32:02 +0100
Subject: [PATCH] Smart Charger Test code
Main code for smart charger testing
---
src/test_charger.c | 167 +++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 167 insertions(+)
create mode 100644 src/test_charger.c
diff --git a/src/test_charger.c b/src/test_charger.c
new file mode 100644
index 0000000..a024361
--- /dev/null
+++ b/src/test_charger.c
@@ -0,0 +1,167 @@
+
+/* Test smart charger with CM730
+ * main behaving as master (PC) to the CM730
+ * Initialization of eeprom, ram, gpio and motion manager (not manager_timer)
+ *
+ * UNCOMMENT DESIRED BLOCKS TO TEST (Debugation with leds)
+ * Scan of smart charger in bus dynamixel v1
+ * Read smart chargers period (1500)
+ * Read smart chargers ID (192)
+ * Write smart charger period
+ * Enable smart charger
+ * Enable motion manager timer
+ * Read battery status of smart charger
+ */
+
+#include "stm32f1xx_hal.h"
+#include "gpio.h"
+#include "eeprom.h"
+#include "ram.h"
+#include "adc_dma.h"
+#include "imu.h"
+#include "darwin_time.h"
+#include "darwin_dyn_master.h"
+#include "darwin_dyn_slave.h"
+#include "motion_manager.h"
+#include "darwin_registers.h"
+
+
+int main(void)
+{
+ uint16_t eeprom_data,period;
+
+ /* initialize the HAL module */
+ HAL_Init();
+ /* initialize EEPROM */
+ EE_Init();
+ // initialize the Dynamixel RAM memory space
+ ram_init();
+ /* initialize the GPIO module */
+ gpio_init();
+ // initialize adc
+// adc_init();
+ // initialize imu
+// imu_init();
+ // initialize time module
+ darwin_time_init();
+ /* initialize the dynamixel slave interface */
+ //darwin_dyn_slave_init();
+ //darwin_dyn_slave_start();
+ /* initialize motion manager module */
+ EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data); //period: 7800us
+ period=eeprom_data&0x00FF;
+ EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data);
+ period+=((eeprom_data&0x00FF)<<8);
+ manager_init(period);
+ //gpio_set_led(LED_4); //azul
+
+ /* unsigned short int address;
+ unsigned short int length;
+ unsigned char data_write;
+ */
+
+ ////SCAN
+ unsigned char num;
+ unsigned char servo_ids[254];
+ dyn_master_scan(&darwin_dyn_master,&num, servo_ids); //dynamixel_master.c
+ if(num==0){
+ gpio_set_led(LED_4);//azul
+ }else{
+ gpio_set_led(LED_3);//Si existen dispositivos conectados -> luz verde
+ }
+
+
+////READ CHARGER STATUS
+ unsigned short int address=DARWIN_SMART_CHARGER_STATUS;
+ unsigned short int length=1;
+ unsigned char data_read;
+ uint8_t error;
+ //unsigned char *pdata = (uint8_t)&data_read;
+ error=darwin_on_read(address,length,&data_read);
+ if(data_read&0x01)
+ gpio_set_led(LED_2); //verde
+
+
+/* ////READ PERIOD SMART CHARGER
+ unsigned short int address=DARWIN_SMART_CHARGER_PERIOD_L;
+ unsigned short int length=2;
+ unsigned char data_read[2];
+ uint8_t error;
+ //unsigned char *pdata = (uint8_t)&data_read;
+ error=darwin_on_read(address,length,data_read);
+ uint16_t dread = data_read[0] + (data_read[1]<<8);
+ if(dread==1500)
+ gpio_set_led(LED_3); //verde
+ else if(dread==0)
+ gpio_set_led(LED_4); //azul
+*/
+
+/* ////READ ID SMART CHARGER
+ unsigned char data_read2;
+ address=DARWIN_SMART_CHARGER_ID;
+ length=1;
+ darwin_on_read(address,length,&data_read2);
+ if(data_read2==192)
+ gpio_set_led(LED_TX); //verde
+ //else if(data_read==0)
+*/
+
+/* ////WRITE PERIOD SMART CHARGER
+ //HAL_Delay(500);
+ unsigned short int address=DARWIN_SMART_CHARGER_PERIOD_L;
+ unsigned short int length=2;
+ unsigned char data_write[2]={0x40, 0x06}; //1600
+ //data_write=0x05DC;
+ darwin_on_write(address,length,data_write); //data_write = data_write[0] address
+ HAL_Delay(500);
+ unsigned char data_read[2];
+ length=2;
+ darwin_on_read(address, length,data_read); //data_read = data_read[0] address
+ uint16_t dread = data_read[0] + (data_read[1]<<8);
+ if(dread==1600)
+ gpio_set_led(LED_3); //verde
+ else if(dread==1500)
+ gpio_set_led(LED_4); //azul
+*/
+
+/* ////ENABLE SMART CHARGER MODULE
+ address=DARWIN_SMART_CHARGER_CNTRL;
+ length=1;
+ uint8_t data_read;
+ ram_read_byte(address,&data_read);
+ //darwin_on_read(address,length,&data_read);
+ if(data_read==1) //Smart charger control = detected
+ gpio_set_led(LED_3); //verde
+ else if(data_read==0) //smart charger NOT detected
+ gpio_set_led(LED_4); //azul
+
+ data_write=SMART_CHARGER_EN;//Smart charger enable
+ darwin_on_write(address,length,&data_write);
+ HAL_Delay(500);
+ darwin_on_read(address,length,&data_read);
+ if(data_read==3) //Smart charger control = detected + enabled
+ gpio_set_led(LED_3); //verde
+*/
+
+/* ////ENABLE MOTION MANAGER TIMER
+ address=DARWIN_MM_CNTRL;
+ data_write=MANAGER_ENABLE;
+ darwin_on_write(address,length,&data_write);
+ HAL_Delay(500);
+*/
+
+/* //Read battery status
+ address=DARWIN_BATT_AVG_TIME_EMPTY_L;
+ length=6;
+ darwin_on_read(address,length,pdata);
+ if(data_read[]==128) //BATTERY_STATUS_L
+ gpio_set_led(LED_4);
+ else
+ gpio_set_led(LED_3);
+*/
+
+ gpio_set_led(LED_RX); //naranja
+
+ while(1);/* main function does not return */
+}
+
--
GitLab