Diff: main.cpp

Revision:

5:4875dac25118

Parent:

3:9ef9b4c66648

--- a/main.cpp	Wed May 02 18:19:22 2018 +0000
+++ b/main.cpp	Sun Jul 15 02:01:57 2018 +0000
@@ -53,8 +53,8 @@
 
 
 Serial       pc(PA_2, PA_3);
-CAN          can1(PB_12, PB_13);  // CAN Rx pin name, CAN Tx pin name
-CAN          can2(PB_8, PB_9);  // CAN Rx pin name, CAN Tx pin name
+CAN          can1(PB_12, PB_13, 1000000);  // CAN Rx pin name, CAN Tx pin name
+CAN          can2(PB_8, PB_9, 1000000);  // CAN Rx pin name, CAN Tx pin name
 
 CANMessage   rxMsg1, rxMsg2;
 CANMessage   txMsg1, txMsg2;
@@ -87,7 +87,6 @@
 void test_control();
 void control();
 
-
 /// CAN Command Packet Structure ///
 /// 16 bit position command, between -4*pi and 4*pi
 /// 12 bit velocity command, between -30 and + 30 rad/s
@@ -210,7 +209,7 @@
     PackAll();
 
     if(counter>100){
-        printf("%.3f %.3f %.3f   %.3f %.3f %.3f\n\r", l1_state.a.p, l1_state.h.p, l1_state.k.p, l2_state.a.p, l2_state.h.p, l2_state.k.p);
+        //printf("%.3f %.3f %.3f   %.3f %.3f %.3f\n\r", l1_state.a.p, l1_state.h.p, l1_state.k.p, l2_state.a.p, l2_state.h.p, l2_state.k.p);
         counter = 0 ;
         }
     
@@ -498,10 +497,12 @@
         
         //spi_data.flags[0] = 0xbeef;
         //spi_data.flags[1] = 0xbeef;
-        //PackAll();
-        //WriteAll();
+        spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
+
     }
-    spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
+    PackAll();
+    WriteAll();
+    
     for(int i = 0; i < DATA_LEN; i++){
         tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
     
@@ -590,20 +591,21 @@
     pack_cmd(&a1_can, l1_control.a); 
     pack_cmd(&a2_can, l2_control.a); 
     pack_cmd(&h1_can, l1_control.h); 
-    pack_cmd(&h2_can, l2_control.h); 
+    pack_cmd(&h2_can, l2_control.h);  
     pack_cmd(&k1_can, l1_control.k); 
     pack_cmd(&k2_can, l2_control.k); 
     WriteAll();
 
-
+    
     // SPI doesn't work if enabled while the CS pin is pulled low
     // Wait for CS to not be low, then enable SPI
-    if(!spi_enabled){   
-        while((spi_enabled==0) && (cs.read() ==0)){wait_us(10);}
-        init_spi();
-        spi_enabled = 1;
-        }
+    //if(!spi_enabled){   
+    //    while((spi_enabled==0) && (cs.read() ==0)){wait_us(10);}
+     //   init_spi();
+     //   spi_enabled = 1;
+      //  }
             
+    loop.attach(&control, .001);
     while(1) {
         counter++;
         can2.read(rxMsg2);
@@ -612,6 +614,13 @@
         unpack_reply(rxMsg1, &l1_state);
         wait_us(10);
 
+        if(counter>1000){
+            printf("%.3f %.3f %.3f   %.3f %.3f %.3f\n\r", l1_state.a.p, l1_state.h.p, l1_state.k.p, l2_state.a.p, l2_state.h.p, l2_state.k.p);
+            counter = 0;
+            }
+        //    PackAll();
+        //    WriteAll();
+        //    }
         }