Gu Jasper
1
Dependencies: mbed-dev-f303 FastPWM3
Diff: main.cpp
- Revision:
- 56:d34e4540ec12
- Parent:
- 52:91a42bd0fe2e
--- a/main.cpp Fri Aug 02 02:39:16 2019 +0000
+++ b/main.cpp Fri Aug 14 07:22:11 2020 +0000
@@ -46,13 +46,18 @@
CANMessage rxMsg;
CANMessage txMsg;
+PositionSensorAM5147 spi(16384, 0.0, NPP); //resolution is 0.02197265625 deg
-SPI drv_spi(PA_7, PA_6, PA_5);
-DigitalOut drv_cs(PA_4);
-//DigitalOut drv_en_gate(PA_11);
-DRV832x drv(&drv_spi, &drv_cs);
+//drv8323 SPI 设置 改为PC12\11\10 SPI3,片选改为PA15
+SPI drv_spi(PC_12, PC_11, PC_10);
-PositionSensorAM5147 spi(16384, 0.0, NPP); //resolution is 0.02197265625 deg
+// GPIOC->AFR[1]&=0xfff000ff;
+// GPIOC->AFR[1]|=0x00055500;
+DigitalOut drv_cs(PA_15);
+
+DRV832x drv(&drv_spi, &drv_cs);
+
+
volatile int count = 0;
volatile int state = REST_MODE;
@@ -142,7 +147,7 @@
//gpio.enable->write(1);
controller.ovp_flag = 0;
controller.init_flag = 0; //HJB added. The flag of fastest way to go to the desire position, state change or CAN time off.
- reset_foc(&controller); // Tesets integrators, and other control loop parameters
+ reset_foc(&controller); // Resets integrators, and other control loop parameters
wait(.001);
controller.i_d_ref = 0;
controller.i_q_ref = 0; // Current Setpoints
@@ -167,7 +172,9 @@
void print_encoder(void){
printf(" Mechanical Angle: %f Electrical Angle: %f Raw: %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition()); //spi.GetMechPosition
+ // printf("%d %d %d \n\r",controller.adc1_raw,controller.adc2_raw,controller.adc3_raw);
//printf("%d\n\r", spi.GetRawPosition());
+ // can.write(txMsg);
wait(.001);
}
@@ -207,7 +214,7 @@
break;
case MOTOR_MODE: // Run torque control
- if(state_change){
+ if(state_change){//Initiate Torque control parameters
enter_torque_mode();
count = 0;
//===============================================HJB added====================================================//
@@ -246,7 +253,7 @@
controller.v_des = 0; //HJB added
}
- torque_control(&controller);
+ torque_control(&controller);//Calculate the desire torque for current loop
commutate(&controller, &observer, &gpio, controller.theta_elec); // Run current loop
controller.timeout++;
@@ -277,7 +284,7 @@
void serial_interrupt(void){
while(pc.readable()){
char c = pc.getc();
- if(c == 27){
+ if(c == 27){ //return to default mode
//===============================================HJB added====================================================//
wait_us(100); //HJB add, to clear fault
drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1); //HJB add, to clear fault
@@ -304,11 +311,11 @@
state = ENCODER_MODE;
state_change = 1;
break;
- case 's':
+ case 's'://
state = SETUP_MODE;
state_change = 1;
break;
- case 'z':
+ case 'z'://set zero position
spi.SetMechOffset(0);
spi.Sample(DT);
wait_us(20);
@@ -396,7 +403,7 @@
gpio.enable->write(1); //enable!
wait_us(100);
- drv.calibrate();
+ drv.calibrate(); //calibrete and initialize the Drv8323 Chip
wait_us(100);
drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1);
wait_us(100);
@@ -419,7 +426,7 @@
TIM1->CCR1 = 0x708*(1.0f);
gpio.enable->write(0);
*/
- reset_foc(&controller); // Reset current controller
+ reset_foc(&controller); // Reset current controller
reset_observer(&observer); // Reset observer
TIM1->CR1 ^= TIM_CR1_UDIS;
//TIM1->CR1 |= TIM_CR1_UDIS; //enable interrupt
@@ -477,7 +484,7 @@
state_change = 1;
- int counter = 0;
+ // int counter = 0;
//===============================================HJB added====================================================//
wait_us(100); //HJB add, to clear fault
drv.write_DCR(0x0, 0x0, 0x0, PWM_MODE_3X, 0x0, 0x0, 0x0, 0x0, 0x1); //HJB add, to clear fault
@@ -485,19 +492,22 @@
//===============================================HJB ended====================================================//
while(1) {
drv.print_faults();
- wait(.1);
+ wait(.1);
//printf("%.4f\n\r", controller.v_bus);
if(state == MOTOR_MODE)
{
//printf(" %.3f %.3f %.3f \n\r", controller.p_des,controller.p_des-controller.theta_mech,controller.v_des-controller.dtheta_mech); //spi.GetMechPosition
- printf(" %.3f %.3f %.3f \n\r", controller.v_des*60/(2*PI),controller.dtheta_mech*60/(2*PI),controller.v_des-controller.dtheta_mech); //spi.GetMechPosition
+ printf(" %.3f %d %d \n\r", controller.v_bus,controller.adc1_raw,controller.adc2_raw); //spi.GetMechPosition
// printf("%.3f %.3f %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance);
//printf("%.3f %.3f %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec);
// printf("%.3f\n\r", controller.dtheta_mech);
wait(.002);
}
-
+// spi.Sample(DT);
+// print_encoder();
+ wait(0.01);
+ // printf(" %d\n\r", drv.read_register(DCR));
/*
if(state == MOTOR_MODE)
{