Diff: main.cpp

Revision:

49:fcfe9f46657b

Parent:

47:55bdc4d5096b

--- a/main.cpp	Thu Jan 09 01:42:00 2020 +0000
+++ b/main.cpp	Wed Apr 14 11:31:33 2021 +0000
@@ -47,12 +47,13 @@
 
 
 
-PositionSensorAM5147 spi(16384, 0.0, NPP);    //14 bits encoder, 21 NPP
+PositionSensorAM5147 spi(16384, 0.0,0);    //14 bits encoder, 21 NPP
 
 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);
     }
 
@@ -218,8 +223,31 @@
                      count = 0;
                      }
                      */
-                     
-            
+                    
+               // if(gpio.brake1->read()==1&&gpio.brake2->read()==1)
+               //{
+                // printf("1\n\r");
+                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 +423,11 @@
 
     
     while(1) {
-
+                if(state==MOTOR_MODE)
+        {
+           printf("%.3f\n\r",controller.theta_mech );
+            wait(1);
+             
+        }
     }
 }