Diff: main.cpp

Revision:

50:9d762c5d05c3

Parent:

47:55bdc4d5096b

--- a/main.cpp	Thu Jan 09 01:42:00 2020 +0000
+++ b/main.cpp	Mon Mar 29 07:08:14 2021 +0000
@@ -52,7 +52,8 @@
 volatile int count = 0;
 volatile int state = REST_MODE;
 volatile int state_change;
-
+volatile int brake_state=1;
+int reg_count=0;
 void onMsgReceived() {
     //msgAvailable = true;
    
@@ -147,6 +148,10 @@
     
 void print_encoder(void){
     printf(" Mechanical Angle:  %f    Electrical Angle:  %f    Raw:  %d\n\r", spi.GetMechPosition(), spi.GetElecPosition(), spi.GetRawPosition());
+     for(reg_count=0;reg_count<=263;reg_count++)
+    {
+        printf("%d  %d\n\r",reg_count,__int_reg[reg_count]);
+    }
     wait(.05);
     }
 
@@ -211,15 +216,27 @@
                 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;
-                     }
-                     */
                      
-            
+                if(brake_state)
+                {
+                 if(gpio.brake1->read()==0||gpio.brake2->read()==0)
+                 {
+                    printf("2\n\r");
+                    controller.i_d_ref = 0;
+                    controller.i_q_ref = 0;
+                    controller.kp = 0;
+                    controller.kd = 0;
+                    controller.t_ff = 0; 
+                    brake_state=0; 
+                     state_change = 0;   
+                    gpio.enable->write(0);
+                    gpio.led->write(0); 
+                 }
+                 }
+                 if(gpio.brake1->read()==1&&gpio.brake2->read()==1)
+                 {
+                     brake_state=1;
+                    }
                 }     
                 break;
             case SETUP_MODE:
@@ -395,6 +412,11 @@
 
     
     while(1) {
-
+                if(state==MOTOR_MODE)
+        {
+           printf("%.3f\n\r",controller.theta_mech );
+            wait(1);
+             
+        }
     }
 }