Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F87079032
control_table.c
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Thu, Oct 10, 11:39
Size
12 KB
Mime Type
text/x-c
Expires
Sat, Oct 12, 11:39 (1 d, 22 h)
Engine
blob
Format
Raw Data
Handle
21489699
Attached To
R6619 Oncilla Motordriver Firmware
control_table.c
View Options
//#include <stdlib.h>
#include "control_table.h"
#include "DEE Emulation 16-bit.h"
#include "error.h"
#include "misc.h"
#include "config.h"
table_field
control_table
[
A_MDRV_TABLE_SIZE
];
uint8
control_table_mode
[
A_MDRV_TABLE_SIZE
];
uint8
protected
;
void
init_control_table
(
void
)
{
unsigned
int
i
;
// set all fields to "reserved"
for
(
i
=
0
;
i
<
A_MDRV_TABLE_SIZE
;
i
++
)
{
control_table_mode
[
i
]
=
0x00
;
}
// set mode bits for implemented table entries
control_table_mode
[
A_MDRV_FIRMWARE_VERSION
]
=
SBCP_P_READABLE
+
SBCP_P_IN_EEPROM
;
control_table_mode
[
A_MDRV_CLASS
]
=
SBCP_P_READABLE
+
SBCP_P_IN_EEPROM
;
control_table_mode
[
A_MDRV_ID
]
=
SBCP_P_READABLE
+
SBCP_P_IN_EEPROM
;
control_table_mode
[
A_MDRV_TIME_OUT_DELAY
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_HIP_CW_ANGLE_LIMIT
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_HIP_CCW_ANGLE_LIMIT
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_KNEE_CW_ANGLE_LIMIT
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_KNEE_CCW_ANGLE_LIMIT
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_SERVO1_CW_ANGLE_LIMIT
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_SERVO1_CCW_ANGLE_LIMIT
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_SERVO2_CW_ANGLE_LIMIT
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_SERVO2_CCW_ANGLE_LIMIT
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_PASSIVE_JOINT_CW_ANGLE_LIMIT
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_PASSIVE_JOINT_CCW_ANGLE_LIMIT
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_HIP_TORQUE_LIMIT
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_KNEE_TORQUE_LIMIT
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_SERVO1_TORQUE_LIMIT
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_SERVO2_TORQUE_LIMIT
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_STATUS_RETURN_LEVEL
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_ALARM_SHUTDOWN
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_CALIBRATION_STATUS
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
+
SBCP_P_IN_EEPROM
+
SBCP_P_PROTECTED
;
control_table_mode
[
A_MDRV_HIP_MOTOR_CONTROL_MODE
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_KNEE_MOTOR_CONTROL_MODE
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_SERVO1_MOTOR_CONTROL_MODE
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_SERVO2_MOTOR_CONTROL_MODE
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_HIP_GOAL_POSITION
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_HIP_PRESENT_DESIRED_POSITION
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_HIP_PRESENT_POSITION
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_KNEE_GOAL_POSITION
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_KNEE_PRESENT_DESIRED_POSITION
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_KNEE_PRESENT_POSITION
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_SERVO1_GOAL_POSITION
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_SERVO1_PRESENT_POSITION
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_SERVO2_GOAL_POSITION
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_SERVO2_PRESENT_POSITION
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_HIP_GOAL_SPEED
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_HIP_PRESENT_DESIRED_SPEED
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_HIP_PRESENT_SPEED
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_KNEE_GOAL_SPEED
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_KNEE_PRESENT_DESIRED_SPEED
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_KNEE_PRESENT_SPEED
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_SERVO1_GOAL_SPEED
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_SERVO1_PRESENT_SPEED
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_SERVO2_GOAL_SPEED
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_SERVO2_PRESENT_SPEED
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_HIP_MAX_ACCELERATION
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_KNEE_MAX_ACCELERATION
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_SERVO1_MAX_ACCELERATION
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_SERVO2_MAX_ACCELERATION
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_HIP_MAX_TORQUE
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_HIP_PRESENT_TORQUE
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_KNEE_MAX_TORQUE
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_KNEE_PRESENT_TORQUE
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_SERVO1_MAX_TORQUE
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_SERVO1_PRESENT_TORQUE
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_SERVO2_MAX_TORQUE
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_SERVO2_PRESENT_TORQUE
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_HIP_SPRING_PRELOAD
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_HIP_SPRING_FORCE
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_HIP_SPRING_DAMPING
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_KNEE_SPRING_PRELOAD
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_KNEE_SPRING_FORCE
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_KNEE_SPRING_DAMPING
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_HIP_CURRENT_CONSUMPTION
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_KNEE_CURRENT_CONSUMPTION
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_SERVO1_CURRENT_CONSUMPTION
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_SERVO2_CURRENT_CONSUMPTION
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_HIP_PID_PGAIN
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_HIP_PID_IGAIN
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_HIP_PID_DGAIN
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_KNEE_PID_PGAIN
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_KNEE_PID_IGAIN
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_KNEE_PID_DGAIN
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_POSITION_UPDATE_INTERVAL
]
=
SBCP_P_READABLE
+
SBCP_P_WRITABLE
;
control_table_mode
[
A_MDRV_ME_Q1_POSITION
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_ME_Q2_POSITION
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_ME_Q3_POSITION
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_TORQUE_SENSOR1
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_TORQUE_SENSOR2
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_TORQUE_SENSOR3
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_TORQUE_SENSOR4
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_TORQUE_SENSOR5
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_TORQUE_SENSOR6
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_ME_Q1_POSITION
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_ME_Q2_POSITION
]
=
SBCP_P_READABLE
;
control_table_mode
[
A_MDRV_ME_Q3_POSITION
]
=
SBCP_P_READABLE
;
// protected parameters can not be changed by default
protected
=
TRUE
;
// read EEPROM content
DataEEInit
();
dataEEFlags
.
val
=
0
;
unsigned
int
firstLaunch
=
1
;
for
(
i
=
0
;
i
<
A_MDRV_TABLE_SIZE
;
i
++
)
{
if
(
control_table_mode
[
i
]
&
SBCP_P_IN_EEPROM
)
{
control_table
[
i
].
i
=
DataEERead
(
i
);
if
(
control_table
[
i
].
u
!=
0xFFFF
)
{
firstLaunch
=
0
;
}
}
else
{
control_table
[
i
].
i
=
0
;
}
}
if
(
firstLaunch
==
1
){
//this is the first time the device is launched,
//we should store some default EEPROM values.
set_control_table
(
A_MDRV_FIRMWARE_VERSION
,
AMARSI_MOTORDRIVER_FIRMWARE_VERSION
);
set_control_table
(
A_MDRV_CLASS
,
SBCP_MY_CLASS
);
set_control_table
(
A_MDRV_ID
,
SBCP_MY_ID
);
set_control_table
(
A_MDRV_TIME_OUT_DELAY
,
1000000
/
FSBCPT
);
set_control_table
(
A_MDRV_ALARM_SHUTDOWN
,
0
);
set_control_table
(
A_MDRV_CALIBRATION_STATUS
,
STATUS_CALIBRATION_INITIAL_UNPERFORMED
);
// some angle limit positions
set_control_table
(
A_MDRV_HIP_CW_ANGLE_LIMIT
,
ANGULAR_RESOLUTION
*
2
/
5
);
set_control_table
(
A_MDRV_HIP_CCW_ANGLE_LIMIT
,
0
);
set_control_table
(
A_MDRV_KNEE_CW_ANGLE_LIMIT
,
ANGULAR_RESOLUTION
*
5
/
5
);
set_control_table
(
A_MDRV_KNEE_CCW_ANGLE_LIMIT
,
0
);
set_control_table
(
A_MDRV_HIP_TORQUE_LIMIT
,
MAX_DUTY
*
256
/
256
);
set_control_table
(
A_MDRV_KNEE_TORQUE_LIMIT
,
MAX_DUTY
*
256
/
256
);
}
/*
* It seems that there is a mismatch between the table value
*/
/**
* Some default vaue for some of the data.
*/
set_control_table
(
A_MDRV_STATUS_RETURN_LEVEL
,
STATUS_COM_RESPOND_TO_READ
+
STATUS_COM_RESPOND_TO_WRITE
+
STATUS_COM_RESPOND_TO_ERROR
);
set_control_table
(
A_MDRV_POSITION_UPDATE_INTERVAL
,
10
);
// in ms
set_control_table
(
A_MDRV_HIP_MOTOR_CONTROL_MODE
,
STATUS_MOTOR_RESET
|
STATUS_MOTOR_RUNTIME_UNCALIBRATED
);
set_control_table
(
A_MDRV_KNEE_MOTOR_CONTROL_MODE
,
STATUS_MOTOR_RESET
|
STATUS_MOTOR_RUNTIME_UNCALIBRATED
);
set_control_table
(
A_MDRV_HIP_PID_PGAIN
,
400
);
set_control_table
(
A_MDRV_HIP_PID_IGAIN
,
20
);
set_control_table
(
A_MDRV_HIP_PID_DGAIN
,
200
);
set_control_table
(
A_MDRV_HIP_SPRING_PRELOAD
,
(
MAX_DUTY
*
256
/
256
));
set_control_table
(
A_MDRV_HIP_SPRING_FORCE
,
M1_INT_TRQ_2_EXT_TRQ
(
100
));
set_control_table
(
A_MDRV_HIP_SPRING_DAMPING
,
500
);
//M1_INT_DAMP_2_EXT_DAMP(500));
set_control_table
(
A_MDRV_HIP_MAX_TORQUE
,
control_table
[
A_MDRV_HIP_TORQUE_LIMIT
].
i
);
set_control_table
(
A_MDRV_HIP_MAX_ACCELERATION
,
M1_INT_ACCEL_2_EXT_ACCEL
(
1000
));
set_control_table
(
A_MDRV_KNEE_PID_PGAIN
,
400
);
set_control_table
(
A_MDRV_KNEE_PID_IGAIN
,
20
);
// 80
set_control_table
(
A_MDRV_KNEE_PID_DGAIN
,
200
);
// 800
set_control_table
(
A_MDRV_KNEE_SPRING_PRELOAD
,
(
MAX_DUTY
*
256
/
256
));
set_control_table
(
A_MDRV_KNEE_SPRING_FORCE
,
M2_INT_TRQ_2_EXT_TRQ
(
100
));
set_control_table
(
A_MDRV_KNEE_SPRING_DAMPING
,
500
);
//M2_INT_DAMP_2_EXT_DAMP(500));
set_control_table
(
A_MDRV_KNEE_MAX_TORQUE
,
control_table
[
A_MDRV_KNEE_TORQUE_LIMIT
].
i
);
set_control_table
(
A_MDRV_KNEE_MAX_ACCELERATION
,
M2_INT_ACCEL_2_EXT_ACCEL
(
1000
));
}
int
get_control_table
(
uint8
id
)
{
return
control_table
[
id
].
i
;
}
// if data is known to be not located in EEPROM-area
void
set_control_table_volatile
(
uint8
id
,
int
value
)
{
control_table
[
id
].
i
=
value
;
}
void
set_control_table
(
uint8
id
,
int
value
)
{
if
(
control_table
[
id
].
i
&
SBCP_P_IN_EEPROM
)
{
DataEEWrite
(
value
,(
unsigned
int
)
id
);
}
control_table
[
id
].
i
=
value
;
}
void
control_table_OR_mask
(
uint8
id
,
int
mask
)
{
control_table
[
id
].
i
|=
mask
;
}
void
control_table_AND_mask
(
uint8
id
,
int
mask
)
{
control_table
[
id
].
i
&=
mask
;
}
Event Timeline
Log In to Comment