Homec4science
Diffusion Oncilla Motordriver Firmware (load-bad-commit) Browse

oncilla-motordriver-fw/src/control_table.c
df700679f13cload-bad-commit

Last Change
Raw File

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;
}
c4science · Help