diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index c648bb56c946816dd4d8656d2c49fcc2b3789e94..244d97f11896cc746854a4c7b4cf1d711d51dbfc 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -617,7 +617,7 @@ typedef enum {
*/
//añadidos
DARWIN_SMART_CHARGER_ID = 0x0247,
- DARWIN_SMART_CHARGER_STATUS = 0x0248,
+ DARWIN_SMART_CHARGER_STATUS = 0x0248,
DARWIN_SMART_CHARGER_LIMIT_CURRENT_L = 0x0249,
DARWIN_SMART_CHARGER_LIMIT_CURRENT_H = 0x024A,
DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L = 0x024B,
diff --git a/src/cm730_fw.c b/src/cm730_fw.c
index e6debd2030e8f6dffaccf018c2dd1a8b0f8337c4..d074c765ddd1f91e06636c2809f33fa476b6acb2 100755
--- a/src/cm730_fw.c
+++ b/src/cm730_fw.c
@@ -6,11 +6,15 @@
#include "imu.h"
#include "darwin_time.h"
#include "darwin_dyn_slave.h"
+#include "darwin_dyn_master.h"
#include "motion_manager.h"
+#include "action.h"
+#include "action_id.h"
int main(void)
{
uint16_t eeprom_data,period;
+ uint8_t i;
/* initialize the HAL module */
HAL_Init();
diff --git a/src/smart_charger.c b/src/smart_charger.c
index daa8ef803b42c9ce9088255ffb6a6c3bf8645785..70de2e7d9e8861e467e41ffc048098053149e7f2 100644
--- a/src/smart_charger.c
+++ b/src/smart_charger.c
@@ -12,12 +12,14 @@ uint16_t smart_charger_period; //smart charger period value
uint16_t counter; //for read access operations
uint16_t smart_charger_count; //counter = sc period / mm period --> Default: (1500ms*1000)/7800us
uint8_t smart_charger_write; //write smart charger signal
+uint8_t init_regs[6]={0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
#define fifosize 32
unsigned char *infifo; //Pointer to fifo position where to store next input data
unsigned char *outfifo; //Pointer to fifo position where is stored the data to be written
unsigned char numdata; //Number of elements stored in the fifo
unsigned char write_fifo[fifosize]; //Vector with data to write (data_write)
+unsigned char current[2];
//---------------------------------------
@@ -32,11 +34,11 @@ void smart_charger_init(uint16_t period_us)
counter = 0;
smart_charger_period = ram_data[DARWIN_SMART_CHARGER_PERIOD_L];
smart_charger_period = (ram_data[DARWIN_SMART_CHARGER_PERIOD_H])<<8;
- smart_charger_count = (smart_charger_period*1000)/period_us;
-
+ smart_charger_count = (smart_charger_period*1000)/period_us;
smart_charger_write = 0x00;
infifo=outfifo=&write_fifo[0];
numdata=0; //fifo empty
+ ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs);
}
inline void smart_charger_set_version(TDynamixelMaster *master)
@@ -113,9 +115,11 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
//Write Battery limit current
if(ram_in_range(DARWIN_SMART_CHARGER_LIMIT_CURRENT_L,address,length) && ram_in_range(DARWIN_SMART_CHARGER_LIMIT_CURRENT_H,address,length))
{
- smart_charger_write = 0x01;
+ current[0]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L-address];
+ current[1]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address];
+
- if(infifo==&write_fifo[fifosize]);
+ /* if(infifo==&write_fifo[fifosize]);
infifo=&write_fifo[0]; //go to first position of fifo
if(numdata<fifosize) //free space in fifo
{
@@ -125,13 +129,15 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
infifo++;
numdata++;
}
+ */
/* *infifo=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L-address]; //copy first byte to fifo
numdata++;
infifo++;
*infifo=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address]; //copy second byte to fifo
numdata++;
infifo++; */
- }
+ // }
+ smart_charger_write = 0x01;
}
}
@@ -139,30 +145,38 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
// motion manager interface function
void smart_charger_process(void)
{
+ gpio_set_led(LED_TX);//amarillo
counter++;
-uint8_t error;
+ uint8_t error;
//Write smart_charger - Battery limit current (EEPROM)
if(smart_charger_detected && smart_charger_enabled && smart_charger_write && counter!=smart_charger_count)
{
- dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2,outfifo);
+ error=dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2, current);
+ if(error==DYN_SUCCESS){
+/* dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2,outfifo);
if(outfifo==&write_fifo[fifosize-1])
outfifo=&write_fifo[0]; //go to first position of fifo
else
outfifo=outfifo+2;
numdata=numdata-2;
if(numdata==0) //empty fifo
- smart_charger_write = 0x00;
+ */
+ smart_charger_write = 0x00;
+ }
}
//Read smart charger - time to empty, time to full and battery_status
if(smart_charger_detected && smart_charger_enabled && counter==smart_charger_count)
{
- counter = 0;
+
error=dyn_master_read_table(dyn_battery_master,smart_charger_id,BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L]);
- if(error==DYN_SUCCESS)
+ if(error!=DYN_SUCCESS){
+ ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs);
+ gpio_set_led(LED_RX); //naranja
+ }else{
gpio_set_led(LED_3);//verde
- else
- gpio_set_led(LED_4);
+ }
+ counter = 0;
}
}
diff --git a/src/test_charger.c b/src/test_charger.c
index a024361a344a8b73eaa6911e0f28144af77db7ba..5a547fceff13e6ed90be00b86f97142b1f391d35 100644
--- a/src/test_charger.c
+++ b/src/test_charger.c
@@ -11,6 +11,8 @@
* Enable smart charger
* Enable motion manager timer
* Read battery status of smart charger
+
+ To test only smart charger module, comment th eother modules in the motion manager timer
*/
#include "stm32f1xx_hal.h"
@@ -60,6 +62,8 @@ int main(void)
unsigned char data_write;
*/
+
+
////SCAN
unsigned char num;
unsigned char servo_ids[254];
@@ -160,6 +164,20 @@ int main(void)
gpio_set_led(LED_3);
*/
+
+//HABILITAR POWER, MOTION MANAGER, SERVOS, DEFINIRLO AL MODULO ACTION, LEVANTAR ROBOT
+// darwin_dyn_master_enable_power();
+// for(i=0;i<31;i++)
+// {
+// manager_set_module(i,MM_ACTION);
+// manager_enable_servo(i);
+// }
+// manager_enable();
+//
+// action_load_page(WALK_READY);
+// action_start_page();
+
+
gpio_set_led(LED_RX); //naranja
while(1);/* main function does not return */