Shao Rui
1
Dependencies: mbed-dev-f303 FastPWM3
main.cpp
- Committer:
- shaorui
- Date:
- 2020-09-15
- Revision:
- 53:32218a36df05
- Parent:
- 52:d4d5e3414865
- Child:
- 54:e201ae25e467
File content as of revision 53:32218a36df05:
/// high-bandwidth 3-phase motor control, for robots
/// Written by benkatz, with much inspiration from bayleyw, nkirkby, scolton, David Otten, and others
/// Hardware documentation can be found at build-its.blogspot.com
/// Written for the STM32F446, but can be implemented on other STM32 MCU's with some further register-diddling
#define REST_MODE 0
#define CALIBRATION_MODE 1
#define MOTOR_MODE 2
#define SETUP_MODE 4
#define ENCODER_MODE 5
#define TEST_TRAJECTORY_MODE 6
#define J_CALIBRATION_MODE 7
#define VERSION_NUM "1.6"
float __float_reg[64]; // Floats stored in flash
//int __int_reg[256]; // Ints stored in flash. Includes position sensor calibration lookup table
int __int_reg[300];
int test1;
int test_jointround_flag=0;
int stop_sign=0;
#include "mbed.h"
#include "PositionSensor.h"
#include "structs.h"
#include "foc.h"
#include "calibration.h"
#include "hw_setup.h"
#include "math_ops.h"
#include "current_controller_config.h"
#include "hw_config.h"
#include "motor_config.h"
#include "stm32f4xx_flash.h"
#include "FlashWriter.h"
#include "user_config.h"
#include "PreferenceWriter.h"
#include "CAN_com.h"
#include "math.h"
#include "MA700Sensor.h"
#include "joint_calibration.h"
PreferenceWriter prefs(7);
//PreferenceWriter prefs(7);
GPIOStruct gpio;
ControllerStruct controller;
COMStruct com;
ObserverStruct observer;
Serial pc(PA_2, PA_3);
CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name, 1000kbps
CANMessage rxMsg;
CANMessage txMsg;
int i=1;//shaorui add
float wucha=0;
float wucha1=0;
PositionSensorAM5147 spi(16384, 0.0, NPP); //14 bits encoder, 21 NPP
PositionSensorMA700 ma700(16384,0.0,NPP); //shaorui add(12/10)
volatile int count = 0;
volatile int state = REST_MODE;
volatile int state_change;
volatile float Joint_init =0; //Joint intial angle
volatile int J_M_flag = 0; // Joint motor angle combine
void onMsgReceived() {
//msgAvailable = true;
can.read(rxMsg);
if((rxMsg.id == CAN_ID)){
controller.timeout = 0;
if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) & (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFC))){
state = MOTOR_MODE;
state_change = 1;
}
else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFD))){
state = REST_MODE;
state_change = 1;
gpio.led->write(0);;
}
else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFE))){
spi.ZeroPosition();
}
else if(state == MOTOR_MODE){
unpack_cmd(rxMsg, &controller);
/*
if(controller.sidebct1!=controller.sidebct)
{
controller.sidebct1=controller.sidebct;
ma700.WriteRegister(&controller);
}
*/
}
pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
can.write(txMsg);
}
}
void enter_menu_state(void){
printf("\n\r\n\r\n\r");
printf(" Commands:\n\r");
wait_us(10);
printf(" m - Motor Mode\n\r");
wait_us(10);
printf(" c - Calibrate Encoder\n\r");
wait_us(10);
printf(" j - Joint Calibrate Encoder\n\r");
wait_us(10);
printf(" t - Joint test Encoder\n\r");
wait_us(10);
printf(" s - Setup\n\r");
wait_us(10);
printf(" e - Display Encoder\n\r");
wait_us(10);
printf(" z - Set Zero Position\n\r");
wait_us(10);
printf(" esc - Exit to Menu\n\r");
wait_us(10);
state_change = 0;
gpio.enable->write(0);
gpio.led->write(0);
}
void enter_setup_state(void){
printf("\n\r\n\r Configuration Options \n\r\n\n");
wait_us(10);
printf(" %-4s %-31s %-5s %-6s %-5s\n\r\n\r", "prefix", "parameter", "min", "max", "current value");
wait_us(10);
printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "b", "Current Bandwidth (Hz)", "100", "2000", I_BW);
wait_us(10);
printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "i", "CAN ID", "0", "127", CAN_ID);
wait_us(10);
printf(" %-4s %-31s %-5s %-6s %-5i\n\r", "m", "CAN Master ID", "0", "127", CAN_MASTER);
wait_us(10);
printf(" %-4s %-31s %-5s %-6s %.1f\n\r", "l", "Torque Limit (N-m)", "0.0", "18.0", TORQUE_LIMIT);
wait_us(10);
printf(" %-4s %-31s %-5s %-6s %d\n\r", "t", "CAN Timeout (cycles)(0 = none)", "0", "100000", CAN_TIMEOUT);
wait_us(10);
printf("\n\r To change a value, type 'prefix''value''ENTER'\n\r i.e. 'b1000''ENTER'\n\r\n\r");
wait_us(10);
state_change = 0;
}
void enter_torque_mode(void){
controller.ovp_flag = 0;
gpio.enable->write(1); // Enable gate drive
reset_foc(&controller); // Tesets integrators, and other control loop parameters
wait(.001);
controller.i_d_ref = 0;
controller.i_q_ref = 0; // Current Setpoints
gpio.led->write(1); // Turn on status LED
state_change = 0;
printf("\n\r Entering Motor Mode \n\r");
}
void calibrate(void){
gpio.enable->write(1); // Enable gate drive
gpio.led->write(1); // Turn on status LED
order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering
calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure
//j_calibrate(&ma700,&spi, &gpio, &controller, &prefs);
//j_calibrate(&ma700,&gpio, &controller, &prefs);
gpio.led->write(0);; // Turn off status LED
wait(.2);
gpio.enable->write(0); // Turn off gate drive
printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r");
state_change = 0;
}
void jocalibrate(void){
gpio.enable->write(1); // Enable gate drive
gpio.led->write(1); // Turn on status LED
//order_phases(&spi, &gpio, &controller, &prefs); // Check phase ordering
//calibrate(&spi, &gpio, &controller, &prefs); // Perform calibration procedure
//j_calibrate(&ma700,&spi, &gpio, &controller, &prefs);
j_calibrate(&ma700,&gpio, &controller, &prefs);
/*************同时设置转子和关节零位置同步****************///暂时不需要,标定结束,按z归零
/*
spi.SetMechOffset(0);
ma700.SetMechOffset(0);
spi.Sample(DT);
ma700.Sample(DT);
wait_us(20);
M_OFFSET = spi.GetMechPosition();
JOINT_M_OFFSET =ma700.GetMechPosition();
if (!prefs.ready()) prefs.open();
prefs.flush(); // Write new prefs to flash
prefs.close();
prefs.load();
spi.SetMechOffset(M_OFFSET);
ma700.SetMechOffset(JOINT_M_OFFSET);
printf("\n\r Saved new zero position: %.4f\n\r\n\r", M_OFFSET);
printf("\n\r Saved new zero position1: %.4f\n\r\n\r", JOINT_M_OFFSET);
*/
/*************同时设置转子和关节零位置同步****************/
gpio.led->write(0); // Turn off status LED
wait(.2);
gpio.enable->write(0); // Turn off gate drive
printf("\n\r Calibration complete. Press 'esc' to return to menu\n\r");
state_change = 0;
}
void jointcalibrate(void){
gpio.enable->write(1); // Enable gate drive
gpio.led->write(1); // Turn on status LED
//joint_calibrate (&ma700,&spi,&gpio,&controller,&prefs); // Perform calibration procedure
gpio.led->write(0); // Turn off status LED
wait(.2);
gpio.enable->write(0);
/************Trajectory Planning******************************/
// enter_torque_mode();
state=MOTOR_MODE;
state_change=1;
//enter_torque_mode();
count = 0;
printf("test\n\r");
controller.p_des=0;
controller.v_des = 1.5f;
controller.kp = 0;
controller.kd = 5.0f;
controller.t_ff = 0;
/*
if((1.0f/GR)* spi.GetMechPosition()<=(2*PI))
{
controller.p_des=0;
controller.v_des = 2.0f;
controller.kp = 0;
controller.kd = 5.0f;
controller.t_ff = 0;
wait(.5);
*
}
************Trajectory Planning*****************************/
// Turn off gate drive
printf("\n\r Joint_Calibration complete. Press 'esc' to return to menu\n\r");
//state_change = 0;
}
void print_encoder(void){
printf(" AS5147Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition()*57.2957795, spi.GetElecPosition()*57.2957795, spi.GetRawPosition());
printf(" MA700:Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", ma700.GetMechPosition()*57.2957795, ma700.GetElecPosition()*57.2957795, ma700.GetRawPosition());
printf(" Joint raw: %f Joint: %f Mech: %f\n\r", controller.theta_joint_raw*57.2957795, controller.theta_joint*57.2957795, controller.theta_mech*57.2957795);
// float m_position=controller.theta_mech*57.2957795;
// float j_position=-controller.theta_mech1*360/(2.0f*PI)-controller.theta_mech*360/(2.0f*PI)*GR;
// float j_position=-(controller.theta_mech1+controller.theta_mech)*2807.49319614;
wait(.5);
}
/// Current Sampling Interrupt ///
/// This runs at 40 kHz, regardless of of the mode the controller is in ///
extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
if (TIM1->SR & TIM_SR_UIF ) {
///Sample current always ///
ADC1->CR2 |= 0x40000000; // Begin sample and conversion
//volatile int delay;
//for (delay = 0; delay < 55; delay++);
spi.Sample(DT);
ma700.Sample(DT);
controller.adc2_raw = ADC2->DR; // Read ADC Data Registers
controller.adc1_raw = ADC1->DR;
controller.adc3_raw = ADC3->DR;
// sample position sensor
controller.theta_elec = spi.GetElecPosition();
// controller.theta_mech = (1.0f/GR)*spi.GetMechPosition();
controller.dtheta_mech = (1.0f/GR)*spi.GetMechVelocity();
controller.dtheta_elec = spi.GetElecVelocity();
controller.v_bus = 0.95f*controller.v_bus + 0.05f*((float)controller.adc3_raw)*V_SCALE;
//////shaorui add for obtaining joint real position
controller.theta_elec1 = ma700.GetElecPosition();
controller.theta_mech1 = (1.0f/GR)*ma700.GetMechPosition();
controller.dtheta_mech1 =(1.0f/GR)*ma700.GetMechVelocity();
controller.dtheta_elec1 = ma700.GetElecVelocity();
///hjb add joint angle
controller.theta_joint_raw= -(ma700.GetMechPosition()+spi.GetMechPosition());
//if(controller.dtheta_mech>0) {controller.theta_joint_raw += PI/180.0f;} //compensate
// else if(controller.dtheta_mech<0) {controller.theta_joint_raw -= PI/180.0f;}
if(controller.theta_joint_raw<0){controller.theta_joint_raw += 2.0f*PI;}
else if(controller.theta_joint_raw>=2.0f*PI){controller.theta_joint_raw -= 2.0f*PI;}
if(controller.theta_joint_raw<0){controller.theta_joint_raw += 2.0f*PI;} //may be still below 0
else if(controller.theta_joint_raw>=2.0f*PI){controller.theta_joint_raw -= 2.0f*PI;}
controller.theta_joint_raw_fil =0.99f*controller.theta_joint_raw_pre + 0.01f*controller.theta_joint_raw ;//filter
controller.theta_joint_raw_pre =controller.theta_joint_raw_fil;
//"rad 0~2pi" ("deg" =-(controller.theta_mech1+controller.theta_mech)*2807.49319614;
//int Ncycle=0;
//float Ncycle_mod = 0.0f;
//float Mech_mod = 0.0f;
controller.Ncycle = controller.theta_joint_raw/(2.0f*PI/GR);
controller.Ncycle_mod = fmod(controller.theta_joint_raw,2.0f*PI/GR);
controller.Mech_mod = fmod(spi.GetMechPosition(),2.0f*PI);
if(controller.Mech_mod<0){controller.Mech_mod += 2.0f*PI;}
else if(controller.Mech_mod>=2.0f*PI){controller.Mech_mod -= 2.0f*PI;}
controller.theta_joint= controller.Ncycle*(2.0f*PI/GR)+controller.Mech_mod/GR; //In the real use, should turn to the theta_mech
if(controller.theta_joint<0){controller.theta_joint += 2.0f*PI;}
else if(controller.theta_joint>=2.0f*PI){controller.theta_joint -= 2.0f*PI;}
if(J_M_flag == 0)
{
if(0<abs(controller.Ncycle_mod*57.2957795-controller.Mech_mod*57.2957795/GR)<2)
{
if(0<abs((controller.theta_joint- controller.theta_joint_raw)*57.2957795)<2)
{
Joint_init = controller.theta_joint- (1.0f/GR)*spi.GetMechPosition();
controller.theta_mech = controller.theta_joint;////controller.theta_joint; // easy way to use, whether available in the shock?
J_M_flag = 1;
}
}
controller.theta_mech = controller.theta_joint_raw;
}
else if(J_M_flag == 1)
{
controller.theta_mech = Joint_init + (1.0f/GR)*spi.GetMechPosition();
}
/////shaorui end//////////////////
/*
controller.c++;
if(controller.c>=20000)
{
controller.cha=controller.init2-controller.init1;
controller.init1=controller.init2;
controller.c=0;
printf("position: %.3f \n\r", controller.cha*360/(2.0f*PI));
}
*/
/// Check state machine state, and run the appropriate function ///
switch(state){
case REST_MODE: // Do nothing
if(state_change){
enter_menu_state();
wucha=0 ; //shaorui add
test_jointround_flag=0;
}
break;
case CALIBRATION_MODE: // Run encoder calibration procedure
if(state_change){
calibrate();
}
break;
case J_CALIBRATION_MODE: // Run encoder calibration procedure
if(state_change){
jocalibrate();
}
break;
case TEST_TRAJECTORY_MODE: // Run encoder calibration procedure
if(state_change){
test_jointround_flag=1;
J_M_flag = 0;
stop_sign=0;
jointcalibrate();
}
break;
case MOTOR_MODE: // Run torque control
if(state_change){
enter_torque_mode();
count = 0;
}
else{
/*
if(controller.v_bus>28.0f){ //Turn of gate drive if bus voltage is too high, to prevent FETsplosion if the bus is cut during regen
gpio.enable->write(0);
controller.ovp_flag = 1;
state = REST_MODE;
state_change = 1;
printf("OVP Triggered!\n\r");
}
*/
torque_control(&controller);
/*
if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
controller.i_d_ref = 0;
controller.i_q_ref = 0;
controller.kp = 0;
controller.kd = 0;
controller.t_ff = 0;
}
*/
commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
controller.timeout += 1;
count++;
/*
if(count == 4000){
printf("%.4f\n\r", controller.dtheta_mech);
count = 0;
}
*/
}
break;
case SETUP_MODE:
if(state_change){
enter_setup_state();
}
break;
case ENCODER_MODE:
print_encoder();
break;
}
}
TIM1->SR = 0x0; // reset the status register
}
char cmd_val[8] = {0};
char cmd_id = 0;
char char_count = 0;
/// Manage state machine with commands from serial terminal or configurator gui ///
/// Called when data received over serial ///
void serial_interrupt(void){
while(pc.readable()){
char c = pc.getc();
if(c == 27){
state = REST_MODE;
state_change = 1;
test_jointround_flag=0;
char_count = 0;
cmd_id = 0;
gpio.led->write(0);;
for(int i = 0; i<8; i++){cmd_val[i] = 0;}
}
if(state == REST_MODE){
switch (c){
case 'c':
state = CALIBRATION_MODE;
state_change = 1;
break;
case 't':
state = TEST_TRAJECTORY_MODE;
state_change = 1;
break;
case 'j':
state = J_CALIBRATION_MODE;
state_change = 1;
break;
case 'm':
state = MOTOR_MODE;
state_change = 1;
break;
case 'e':
state = ENCODER_MODE;
state_change = 1;
break;
case 's':
state = SETUP_MODE;
state_change = 1;
break;
case 'z':
spi.SetMechOffset(0);
ma700.SetMechOffset(0);
spi.Sample(DT);
ma700.Sample(DT);
wait_us(20);
M_OFFSET = spi.GetMechPosition();
JOINT_M_OFFSET = ma700.GetMechPosition();
if (!prefs.ready()) prefs.open();
prefs.flush(); // Write new prefs to flash
prefs.close();
prefs.load();
spi.SetMechOffset(M_OFFSET);
ma700.SetMechOffset(JOINT_M_OFFSET);
J_M_flag=0;
printf("\n\r Saved new M-J J-J zero position: %.4f %.4f\n\r\n\r", M_OFFSET, JOINT_M_OFFSET);
/*
for(int i=0;i<300;i++)
{
printf("%.3d %.3d\n\r",i,__int_reg[i] );
}
for(int j=0;j<64;j++)
{
printf("%.3d %.3f\n\r",j,__float_reg[j] );
}
*/
break;
}
}
else if(state == SETUP_MODE){
if(c == 13){
switch (cmd_id){
case 'b':
I_BW = fmaxf(fminf(atof(cmd_val), 2000.0f), 100.0f);
break;
case 'i':
CAN_ID = atoi(cmd_val);
break;
case 'm':
CAN_MASTER = atoi(cmd_val);
break;
case 'l':
TORQUE_LIMIT = fmaxf(fminf(atof(cmd_val), 18.0f), 0.0f);
break;
case 't':
CAN_TIMEOUT = atoi(cmd_val);
break;
default:
printf("\n\r '%c' Not a valid command prefix\n\r\n\r", cmd_id);
break;
}
if (!prefs.ready()) prefs.open();
prefs.flush(); // Write new prefs to flash
prefs.close();
prefs.load();
state_change = 1;
char_count = 0;
cmd_id = 0;
for(int i = 0; i<8; i++){cmd_val[i] = 0;}
}
else{
if(char_count == 0){cmd_id = c;}
else{
cmd_val[char_count-1] = c;
}
pc.putc(c);
char_count++;
}
}
else if (state == ENCODER_MODE){
switch (c){
case 27:
state = REST_MODE;
state_change = 1;
break;
}
}
}
}
int main() {
controller.v_bus = V_BUS;
controller.mode = 0;
controller.sidebct1=0;
Init_All_HW(&gpio); // Setup PWM, ADC, GPIO
wait(.1);
gpio.enable->write(1);
TIM1->CCR3 = PWM_ARR*(1.0f); // Write duty cycles
TIM1->CCR2 = PWM_ARR*(1.0f);
TIM1->CCR1 = PWM_ARR*(1.0f);
zero_current(&controller.adc1_offset, &controller.adc2_offset); // Measure current sensor zero-offset
gpio.enable->write(0);
reset_foc(&controller); // Reset current controller
TIM1->CR1 ^= TIM_CR1_UDIS;
//TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
wait(.1);
NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 0); // commutation > communication
NVIC_SetPriority(CAN1_RX0_IRQn, 3);
// If preferences haven't been user configured yet, set defaults
prefs.load(); // Read flash
can.filter(CAN_ID , 0xFFF, CANStandard, 0);
txMsg.id = CAN_MASTER;
txMsg.len = 6;
rxMsg.len = 8;
can.attach(&onMsgReceived);
if(isnan(E_OFFSET)){E_OFFSET = 0.0f;}
if(isnan(M_OFFSET)){M_OFFSET = 0.0f;}
spi.SetElecOffset(E_OFFSET); // Set position sensor offset
spi.SetMechOffset(M_OFFSET);
//pc.baud(460800);//pc.baud(921600);
pc.baud(115000);
//=====benk 2020 last version===//
spi.Sample(1.0f);
printf("MP: %.3f\n\r",spi.GetMechPosition());
if(spi.GetMechPosition() > 2.0f*PI){spi.SetMechOffset(M_OFFSET+2.0f*PI);} // now zeroes to +- 30 degrees about nominal, independent of rollover point
else if (spi.GetMechPosition() < 0){spi.SetMechOffset(M_OFFSET-2.0f*PI);}
//=====benk 2020 last version===//
if(isnan(JOINT_E_OFFSET)){JOINT_E_OFFSET = 0.0f;}
if(isnan(JOINT_M_OFFSET)){JOINT_M_OFFSET = 0.0f;}
ma700.SetElecOffset(JOINT_E_OFFSET); // Set position sensor offset
ma700.SetMechOffset(JOINT_M_OFFSET);
//=====benk 2020 last version===//
ma700.Sample(1.0f);
printf("JP: %.3f\n\r",ma700.GetMechPosition());
if(ma700.GetMechPosition() > 2.0f*PI){ma700.SetMechOffset(JOINT_M_OFFSET+2.0f*PI);} // now zeroes to +- 30 degrees about nominal, independent of rollover point
else if (ma700.GetMechPosition() < 0){ma700.SetMechOffset(JOINT_M_OFFSET-2.0f*PI);}
spi.Sample(1.0f);
ma700.Sample(1.0f);
printf("MP: %.3f JP: %.3f\n\r",spi.GetMechPosition(),ma700.GetMechPosition());
//=====benk 2020 last version===//
int lut[128] = {0};
int joint[128]={0}; //shaorui
memcpy(&lut, &ENCODER_LUT, sizeof(lut));
spi.WriteLUT(lut);
memcpy(&joint, &ENCODER_JOINT , sizeof(joint));
ma700.WriteLUT(joint);
// set serial baud rate
wait(.01);
printf("MP: %.3f JP: %.3f\n\r",spi.GetMechPosition(),ma700.GetMechPosition());
pc.printf("\n\r\n\r QXSLAB Joint\n\r\n\r");
wait(.01);
printf("\n\r Debug Info:\n\r");
printf(" Firmware Version: %s\n\r", VERSION_NUM);
printf(" ADC1 Offset: %d ADC2 Offset: %d\n\r", controller.adc1_offset, controller.adc2_offset);
printf(" Position Sensor Electrical Offset: %.4f\n\r", E_OFFSET*57.2957795);
printf(" Output Zero Position: %.4f\n\r", M_OFFSET*57.2957795);
printf(" CAN ID: %d\n\r", CAN_ID);
controller.theta_joint_raw_pre=-(ma700.GetMechPosition()+spi.GetMechPosition()); //hjb added, for joint encoder filter
pc.attach(&serial_interrupt); // attach serial interrupt
J_M_flag = 0;
//state_change = 1;
while(1) {
// wait(.1);
if(state == MOTOR_MODE)
{
if(test_jointround_flag==1)
{
if(controller.theta_joint_raw*57.2957795<=1)
{
//if(stop_sign==0)
//{
controller.v_des = 0;
wait(1);
controller.p_des=0;
controller.v_des = 1.0f;
controller.kp = 0;
controller.kd = 2.0f;
controller.t_ff = 0;
wait(.5);
// }
/*
else
{
test_jointround_flag=0;
controller.v_des =0;
}
*/
}
else if(controller.theta_joint_raw*57.2957795>=(359))
{
//stop_sign=1;
controller.v_des = 0;
wait(1);
controller.p_des=0;
controller.v_des = -1.0f;
controller.kp = 0;
controller.kd = 2.0f;
controller.t_ff = 0;
wait(.5);
printf("test position:%.3f\n\r",(1.0f/GR)* spi.GetMechPosition());
}
}
//wait(.1);
// printf("%.3f\n\r",(1.0f/GR)* spi.GetMechPosition());
// printf("%.3d, %.3d\n\r",test_jointround_flag, stop_sign);
//printf("BCT: %.3x zzz: %.3x etxy: %.3x \n\r",ma700.Gettest(),ma700.Gettest1(),ma700.Gettest2());
// float joint_mech_position=-(controller.theta_mech*360/(2.0f*PI)*GR+controller.theta_mech1*360/(2.0f*PI));
// wucha1=(controller.theta_mech-controller.theta_mech1)*360/(2.0f*PI);
//wucha1=controller.theta_mech*360/(2.0f*PI)-joint_mech_position;
//wucha+=abs(wucha1);
//printf("M: %.3f J: %.3f E: %.3f EA: %.3f \n\r",controller.theta_mech*360/(2.0f*PI),controller.theta_mech1*360/(2.0f*PI),wucha1,float(wucha/i)) ;
// printf("M: %.3f J: %.3f E: %.3f EA: %.3f \n\r",controller.theta_mech*360/(2.0f*PI),joint_mech_position,wucha1,float(wucha/i)) ;
//printf("m_position: %.3f\n\r",controller.theta_mech*360/(2.0f*PI)*GR);
//printf("j_position: %.3f\n\r",controller.theta_mech1*360/(2.0f*PI));
// float m_position=controller.theta_mech*57.2957795;
// float j_position=-controller.theta_mech1*360/(2.0f*PI)-controller.theta_mech*360/(2.0f*PI)*GR;
// float j_position=-(controller.theta_mech1+controller.theta_mech)*2807.49319614;
// float j_position=-controller.theta_mech1*57.2957795;
//printf("m:%.3f\n\r,j:%.3f\n\r",m_position,j_position);
if(count >= 4000){
// printf("J: %.3f Mec: %.3f Jerr: %.3f JVerr: %.3f Kp: %.3f Kd: %.3f \n\r",controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, (controller.p_des - controller.theta_mech)*57.2957795,(controller.v_des - controller.dtheta_mech)*57.2957795, controller.kp,controller.kd);
printf("Jraw:%.3f J: %.3f Mec: %.3f N: %.3d Nmod: %.3f Mmod: %.3f \n\r",controller.theta_joint_raw*57.2957795,controller.theta_joint*57.2957795, controller.theta_mech*57.2957795, controller.Ncycle,controller.Ncycle_mod*57.2957795,controller.Mech_mod*57.2957795);
printf("Pdes: %.3f Vdes: %.3f Kp: %.3f Kd: %.3f Tff: %.3f\n\r",controller.p_des*57.2957795, controller.v_des*57.2957795, controller.kp,controller.kd,controller.t_ff);
printf("Prel: %.3f Vrel: %.3f T: %.3f \n\r",controller.theta_mech*57.2957795, controller.dtheta_mech*57.2957795, controller.i_q_filt*KT_OUT);
count = 0;
}
i++;
//wait(.5);
}
}
}