Shao Rui
1
Dependencies: mbed-dev-f303 FastPWM3
Diff: main.cpp
- Revision:
- 48:1b51771c3647
- Parent:
- 47:55bdc4d5096b
--- a/main.cpp Sat Dec 07 08:01:06 2019 +0000
+++ b/main.cpp Fri Feb 07 11:31:37 2020 +0000
@@ -8,13 +8,17 @@
#define MOTOR_MODE 2
#define SETUP_MODE 4
#define ENCODER_MODE 5
-
+#define JOINT_CALIBRATION_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[256]; // Ints stored in flash. Includes position sensor calibration lookup table
+int __int_reg[300];
+int test1;
+int joint_flag=0;
+int stop_sign=0;
#include "mbed.h"
#include "PositionSensor.h"
#include "structs.h"
@@ -30,9 +34,12 @@
#include "user_config.h"
#include "PreferenceWriter.h"
#include "CAN_com.h"
+#include "math.h"
+#include "MA700Sensor.h"
+#include "joint_calibration.h"
+PreferenceWriter prefs(6);
+//PreferenceWriter prefs(7);
-
-PreferenceWriter prefs(6);
GPIOStruct gpio;
ControllerStruct controller;
@@ -40,14 +47,15 @@
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;
@@ -73,7 +81,15 @@
}
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);
}
@@ -88,6 +104,10 @@
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");
@@ -138,6 +158,8 @@
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
@@ -145,6 +167,79 @@
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);
+ 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);
+
+ /*************同时设置转子和关节零位置同步****************/
+ 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 );
+
+ /*************同时设置转子和关节零位置同步****************/
+
+ /************Trajectory Planning******************************/
+
+
+ // enter_torque_mode();
+ state=MOTOR_MODE;
+ state_change=1;
+ //enter_torque_mode();
+ count = 0;
+ printf("test\n\r");
+
+ /*
+ 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(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
wait(.05);
@@ -162,19 +257,37 @@
controller.adc2_raw = ADC2->DR; // Read ADC Data Registers
controller.adc1_raw = ADC1->DR;
controller.adc3_raw = ADC3->DR;
- spi.Sample(DT); // sample position sensor
+ spi.Sample(DT);
+ ma700.Sample(DT); // 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.init2=controller.theta_mech1 = ma700.GetMechPosition();
+ controller.dtheta_mech1 =ma700.GetMechVelocity();
+ controller.dtheta_elec1 = ma700.GetElecVelocity();
+ /////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
}
break;
@@ -184,6 +297,23 @@
}
break;
+ case J_CALIBRATION_MODE: // Run encoder calibration procedure
+ if(state_change){
+ jocalibrate();
+
+ }
+ break;
+
+ case JOINT_CALIBRATION_MODE: // Run encoder calibration procedure
+ if(state_change){
+ joint_flag=1;
+ stop_sign=0;
+ jointcalibrate();
+
+ }
+ break;
+
+
case MOTOR_MODE: // Run torque control
if(state_change){
enter_torque_mode();
@@ -200,7 +330,8 @@
}
*/
- torque_control(&controller);
+ torque_control(&controller);
+ /*
if((controller.timeout > CAN_TIMEOUT) && (CAN_TIMEOUT > 0)){
controller.i_d_ref = 0;
controller.i_q_ref = 0;
@@ -208,6 +339,7 @@
controller.kd = 0;
controller.t_ff = 0;
}
+ */
commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
controller.timeout += 1;
@@ -259,6 +391,16 @@
state = CALIBRATION_MODE;
state_change = 1;
break;
+
+ case 't':
+ state = JOINT_CALIBRATION_MODE;
+ state_change = 1;
+ break;
+ case 'j':
+ state = J_CALIBRATION_MODE;
+ state_change = 1;
+ break;
+
case 'm':
state = MOTOR_MODE;
state_change = 1;
@@ -271,17 +413,31 @@
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);
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 );
+ 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;
}
@@ -345,6 +501,7 @@
controller.v_bus = V_BUS;
controller.mode = 0;
+ controller.sidebct1=0;
Init_All_HW(&gpio); // Setup PWM, ADC, GPIO
wait(.1);
@@ -359,7 +516,7 @@
//TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
wait(.1);
- NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 2); // commutation > communication
+ NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 0); // commutation > communication
NVIC_SetPriority(CAN1_RX0_IRQn, 3);
can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
@@ -368,16 +525,17 @@
txMsg.len = 6;
rxMsg.len = 8;
can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
-
prefs.load(); // Read flash
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);
int lut[128] = {0};
+ int joint[128]={0};
memcpy(&lut, &ENCODER_LUT, sizeof(lut));
- spi.WriteLUT(lut); // Set potision sensor nonlinearity lookup table
-
+ spi.WriteLUT(lut);
+ memcpy(&joint, &ENCODER_JOINT , sizeof(joint));
+ spi.WriteLUT(joint);
pc.baud(115200);//pc.baud(921600); // set serial baud rate
wait(.01);
pc.printf("\n\r\n\r HobbyKing Cheetah\n\r\n\r");
@@ -391,10 +549,77 @@
pc.attach(&serial_interrupt); // attach serial interrupt
- state_change = 1;
+ //state_change = 1;
- while(1) {
-
+ while(1) {
+ wait(.1);
+ if(state == MOTOR_MODE)
+ {
+ if(joint_flag==1)
+ {
+ if((1.0f/GR)* spi.GetMechPosition()<=0.01)
+ {
+ //if(stop_sign==0)
+ //{
+ controller.v_des = 0;
+ wait(1);
+ controller.p_des=0;
+ controller.v_des = 1.5f;
+ controller.kp = 0;
+ controller.kd = 5.0f;
+ controller.t_ff = 0;
+ wait(.5);
+ // }
+ /*
+ else
+ {
+ joint_flag=0;
+ controller.v_des =0;
+
+ }
+ */
+ }
+ else if((1.0f/GR)* spi.GetMechPosition()>=(2*PI))
+ {
+ //stop_sign=1;
+ controller.v_des = 0;
+ wait(1);
+ controller.p_des=0;
+ controller.v_des = -1.5f;
+ controller.kp = 0;
+ controller.kd = 5.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",joint_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*57.2957795-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);
+
+
+ i++;
+ wait(.5);
+
+ }
+
+
}
}