Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
B
bioloid_stm32_fw
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
humanoides
bioloid_stm32_fw
Commits
2fc21611
Commit
2fc21611
authored
9 years ago
by
Sergi Hernandez
Browse files
Options
Downloads
Patches
Plain Diff
Used a global variable for the resulting angles in the action module.
parent
a4ec99c7
No related branches found
No related tags found
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
src/action.c
+25
-24
25 additions, 24 deletions
src/action.c
src/bioloid_stm32.c
+32
-0
32 additions, 0 deletions
src/bioloid_stm32.c
with
57 additions
and
24 deletions
src/action.c
+
25
−
24
View file @
2fc21611
#include
"action.h"
#include
"motion_pages.h"
#include
"motion_manager.h"
#include
"ram.h"
#define SPEED_BASE_SCHEDULE 0x00
...
...
@@ -12,7 +14,6 @@ TPage action_current_page;
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
...
...
@@ -135,10 +136,10 @@ void action_load_next_step(void)
{
angle
=
action_current_page
.
steps
[
action_current_step_index
].
position
[
i
];
if
(
angle
==
0x5A00
)
// bigger than 180
target_angles
=
action_
current_angles
[
i
];
target_angles
=
current_angles
[
i
];
else
target_angles
=
angle
<<
9
;
action_moving_angles
[
i
]
=
target_angles
-
action_
current_angles
[
i
];
action_moving_angles
[
i
]
=
target_angles
-
current_angles
[
i
];
if
(
action_end
)
next_angles
=
target_angles
;
else
...
...
@@ -153,8 +154,8 @@ void action_load_next_step(void)
next_angles
=
next_angle
<<
9
;
}
// 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
)))
if
(((
current_angles
[
i
]
<
target_angles
)
&&
(
target_angles
<
next_angles
))
||
((
current_angles
[
i
]
>
target_angles
)
&&
(
target_angles
>
next_angles
)))
dir_change
=
0x00
;
else
dir_change
=
0x01
;
...
...
@@ -224,7 +225,7 @@ void action_init(uint16_t period)
{
// angle variables
action_moving_angles
[
i
]
=
0
;
// fixed point 48|16 format
action_start_angles
[
i
]
=
action_
current_angles
[
i
];
action_start_angles
[
i
]
=
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
...
...
@@ -281,7 +282,7 @@ void action_start_page(void)
uint8_t
i
;
for
(
i
=
0
;
i
<
PAGE_MAX_NUM_SERVOS
;
i
++
)
action_start_angles
[
i
]
=
action_
current_angles
[
i
];
action_start_angles
[
i
]
=
current_angles
[
i
];
action_stop
=
0x00
;
action_current_time
=
0
;
action_section_time
=
0
;
...
...
@@ -326,7 +327,7 @@ void action_process(void)
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
];
current_angles
[
i
]
=
action_start_angles
[
i
]
+
accel_angles
[
i
];
/* update of the state */
if
(
!
action_zero_speed_finish
[
i
])
{
...
...
@@ -334,15 +335,15 @@ void action_process(void)
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
];
action_start_angles
[
i
]
=
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
];
action_start_angles
[
i
]
=
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_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
;
...
...
@@ -356,7 +357,7 @@ void action_process(void)
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
];
current_angles
[
i
]
=
action_start_angles
[
i
]
+
accel_angles
[
i
];
}
state
=
ACTION_PRE
;
}
...
...
@@ -366,21 +367,21 @@ void action_process(void)
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_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_angles
[
i
]
=
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
);
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_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
];
}
}
...
...
@@ -392,7 +393,7 @@ void action_process(void)
{
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_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
;
...
...
@@ -407,15 +408,15 @@ void action_process(void)
{
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
);
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_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_angles
[
i
]
=
current_angles
[
i
];
action_start_speed
[
i
]
=
current_speed
[
i
];
}
/* load the next step */
...
...
@@ -437,7 +438,7 @@ void action_process(void)
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
];
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
;
...
...
@@ -459,11 +460,11 @@ void action_process(void)
{
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
);
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_angles
[
i
]
=
action_start_angles
[
i
]
+
(
main_angles
[
i
]
*
action_current_time
)
/
action_section_time
;
current_speed
[
i
]
=
action_main_speed
[
i
];
}
}
...
...
@@ -491,7 +492,7 @@ void action_process(void)
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
];
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
;
...
...
This diff is collapsed.
Click to expand it.
src/bioloid_stm32.c
+
32
−
0
View file @
2fc21611
...
...
@@ -3,8 +3,40 @@
int32_t
main
(
void
)
{
// uint16_t eeprom_data,period;
/* initialize EEPROM */
// EE_Init();
// initialize the Dynamixel RAM memory space
// ram_init();
/* initialize the 1ms system timer */
// time_init();
/* initialize the gpio */
gpio_init
();
/* initialize the dynamixel master interface */
// dyn_master_init();
// dyn_master_set_timeout(20);
// /* 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 the IMU */
// imu_init();
// initialize the Analog to digital converter
// adc_init();
// initialize motion manager
// EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data);
// period=eeprom_data&0x00FF;
// EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data);
// period+=((eeprom_data&0x00FF)<<8);
// manager_init(period);
/* initialize communications module */
// comm_init();
// comm_start();
while
(
1
);
}
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment