Hello,
I am using XMC4500 relax kit to program and run MIT Cheetah motor AK80-9. The code is correct enough that the motor shows the green light when but when I try to debug and run the motor doesnot respond correctly. from 20 tries motor might respond once or twice and shows the green light. I have checked the c/c++ settings, motor CAN id.
Am i missing something or what could have gone wrong.
I am using DAVE 4.4.2
The code:
/*
* main.c
*
* Created on: 2024 Feb 26 16:44:40
* Author: Rajal Nagwekar
*/
#include "DAVE.h" //Declarations from DAVE Code Generation (includes SFR declaration)
#include <stdio.h>
#include <math.h>
char str[200];
bool sys_on = 0;
int start = 0;
#define P_MIN -12.5f
#define P_MAX 12.5f
#define V_MIN -50.0f
#define V_MAX 50.0f
#define KP_MIN 0.0f
#define KP_MAX 500.0f
#define KD_MIN 0.0f
#define KD_MAX 5.0f
#define T_MIN -18.0f
#define T_MAX 18.0f
//initializing motor input values
float p_int = 0.0;
float v_int = 0.0;
float kp_int = 0.0;
float kd_int = 0.0;
float t_int = 0.0;
//t_int (+ve) - down, t_int (-ve) - up
//intializing variables to read motor output
float p_out = 0.0f;
float v_out = 0.0f;
float t_out = 0.0f;
int id;
float p1,v1,t1,p2,v2,t2,p1o,v1o,t1o,p2o,v2o,t2o,po,vo,to;
uint8_t motor_on[8] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC };
uint8_t zero_position[8] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE };
uint8_t motor_off[8] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD };
uint8_t *update_data;
CAN_NODE_STATUS_t mo_tranmit_status;
unsigned int float_to_uint(float x, float x_min, float x_max, int bits) {
float span = x_max - x_min;
float offset = x_min;
return (unsigned int) ((x - offset) * ((float) ((1 << bits) - 1)) / span);
}
float uint_to_float(unsigned int x_int, float x_min, float x_max, int bits) {
/// converts unsigned int to float, given range and number of bits ///
float span = x_max - x_min;
float offset = x_min;
return ((float) x_int) * span / ((float) ((1 << bits) - 1)) + offset;
}
void motorInitialize(int x) {
update_data = motor_on;
CAN_NODE_MO_UpdateData((&Request_Node)->lmobj_ptr[x], update_data);
mo_tranmit_status = CAN_NODE_MO_Transmit((&Request_Node)->lmobj_ptr[x]);
}
void setZero(int x) {
update_data = zero_position;
CAN_NODE_MO_UpdateData((&Request_Node)->lmobj_ptr[x], update_data);
mo_tranmit_status = CAN_NODE_MO_Transmit((&Request_Node)->lmobj_ptr[x]);
}
//Transmission message to motor
void motorStop(int x) {
update_data = motor_off;
CAN_NODE_MO_UpdateData((&Request_Node)->lmobj_ptr[x], update_data);
CAN_NODE_MO_Transmit((&Request_Node)->lmobj_ptr[x]);
}
void transmitMessage(int x, float *p, float *v, float *t) {
*p = p_int;
*v = v_int;
*t = t_int;
//check if input value is within limits
float p_des = fminf(fmaxf(P_MIN, p_int), P_MAX);
float v_des = fminf(fmaxf(V_MIN, v_int), V_MAX);
float kp = fminf(fmaxf(KP_MIN, kp_int), KP_MAX);
float kd = fminf(fmaxf(KD_MIN, kd_int), KD_MAX);
float t_ff = fminf(fmaxf(T_MIN, t_int), T_MAX);
//float to unsigned integer conversion
unsigned int p_uint = float_to_uint(p_des, P_MIN, P_MAX, 16);
unsigned int v_uint = float_to_uint(v_des, V_MIN, V_MAX, 12);
unsigned int kp_uint = float_to_uint(kp, KP_MIN, KP_MAX, 12);
unsigned int kd_uint = float_to_uint(kd, KD_MIN, KD_MAX, 12);
unsigned int t_uint = float_to_uint(t_ff, T_MIN, T_MAX, 12);
//assign uint values to can data fo transmission
XMC_CAN_MO_t *transmit_objecct = (&Request_Node)->lmobj_ptr[x]->mo_ptr;
transmit_objecct->can_data_byte[0] = p_uint >> 8;
transmit_objecct->can_data_byte[1] = p_uint & 0xFF;
transmit_objecct->can_data_byte[2] = v_uint >> 4;
transmit_objecct->can_data_byte[3] = ((v_uint & 0xF) << 4 | (kp_uint >> 8));
transmit_objecct->can_data_byte[4] = kp_uint & 0xFF;
transmit_objecct->can_data_byte[5] = kd_uint >> 4;
transmit_objecct->can_data_byte[6] = ((kd_uint & 0xF) << 4 | (t_uint >> 8));
transmit_objecct->can_data_byte[7] = t_uint & 0xFF;
CAN_NODE_MO_Init((&Request_Node)->lmobj_ptr[x]);
//{0x7F, 0XFF, 0x7F, 0XF0, 0x00, 0x00, 0x07, 0xFF}
CAN_NODE_MO_Transmit((&Request_Node)->lmobj_ptr[x]);
p_uint = (transmit_objecct->can_data_byte[0] << 😎
| transmit_objecct->can_data_byte[1];
v_uint = (transmit_objecct->can_data_byte[2] << 4)
| (transmit_objecct->can_data_byte[3] >> 4);
kp_uint = ((transmit_objecct->can_data_byte[3] & 0xF) << 😎
| transmit_objecct->can_data_byte[4];
kd_uint = (transmit_objecct->can_data_byte[5] << 4)
| (transmit_objecct->can_data_byte[6] >> 4);
t_uint = ((transmit_objecct->can_data_byte[6] & 0xF) << 😎
| transmit_objecct->can_data_byte[7];
p_des = uint_to_float(p_uint, P_MIN, P_MAX, 16);
v_des = uint_to_float(v_uint, V_MIN, V_MAX, 12);
kp = uint_to_float(kp_uint, KP_MIN, KP_MAX, 12);
kd = uint_to_float(kd_uint, KD_MIN, KD_MAX, 12);
t_ff = uint_to_float(t_uint, T_MIN, T_MAX, 12);
}
void receiveMessage(int x, int *id, float *p, float *v, float *t) {
XMC_CAN_MO_t *receive_objecct = (&Request_Node)->lmobj_ptr[x]->mo_ptr;
CAN_NODE_MO_Receive((&Request_Node)->lmobj_ptr[x]);
// CAN_NODE_MO_ReceiveData((&Request_Node)->lmobj_ptr[1]);
*id = receive_objecct->can_data_byte[0];
unsigned int p_out = (receive_objecct->can_data_byte[1] << 😎
| receive_objecct->can_data_byte[2];
unsigned int v_out = (receive_objecct->can_data_byte[3] << 4)
| (receive_objecct->can_data_byte[4] >> 4);
unsigned int i_out = ((receive_objecct->can_data_byte[4] & 0xF) << 😎
| receive_objecct->can_data_byte[5];
*p = uint_to_float(p_out, P_MIN, P_MAX, 16);
*v = uint_to_float(v_out, V_MIN, V_MAX, 12);
*t = uint_to_float(i_out, T_MIN, T_MAX, 12);
/* sprintf(str,"%d,%.2f,%.2f,%.2f\r\n",id,);
SEGGER_RTT_WriteString(0, str);*/
}
/**
* @brief main() - Application entry point
*
* <b>Details of function</b><br>
* This routine is the application entry point. It is invoked by the device startup code. It is responsible for
* invoking the APP initialization dispatcher routine - DAVE_Init() and hosting the place-holder for user application
* code.
*/
int main(void)
{
DAVE_STATUS_t status;
status = DAVE_Init(); /* Initialization of DAVE APPs */
if (status != DAVE_STATUS_SUCCESS)
{
/* Placeholder for error handler code. The while loop below can be replaced with an user error handler. */
XMC_DEBUG("DAVE APPs initialization failed\n");
while(1U)
{
}
}
motorInitialize(0);
setZero(0);
start = 1;
/* Placeholder for user application code. The while loop below can be replaced with user application code. */
while(start)
{
p_int = p_int + 0.01;
transmitMessage(0, &p1, &v1, &t1);
receiveMessage(1,&id,&p1o,&v1o,&t1o);
sprintf(str,"%d,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",id,p1,v1,t1,p1o,v1o,t1o);
SEGGER_RTT_WriteString(0, str);
}
}
Show Less
Solved