Revision 11:16d807d6b9c5, committed 2018-03-07

Comitter:

benkatz

Date:

Wed Mar 07 19:25:49 2018 +0000

Parent:

10:4b7f2653fb45

Commit message:

Seems to work;

Changed in this revision

--- a/cube.cpp	Sun Feb 04 04:37:05 2018 +0000
+++ b/cube.cpp	Wed Mar 07 19:25:49 2018 +0000
@@ -173,6 +173,15 @@
     return 0;
 }
 
+int check_double_move(int f1, int f2)
+{
+    return (f1 == 0 && f2 == 3) ||
+           (f2 == 0 && f1 == 3) ||
+           (f2 == 1 && f1 == 4) ||
+           (f2 == 1 && f1 == 4) ||
+           (f2 == 2 && f1 == 5) ||
+           (f2 == 2 && f1 == 5);
+}
 
 int states[6] = {0,0,0,0,0,0};
 int move_counts[6] = {0,0,0,0,0,0};
@@ -207,7 +216,7 @@
         cmd->set_and(0,cmd->face); // and off
 
         // check if done
-        if(move_counts[cmd->face] > n_moves)
+        if(move_counts[cmd->face] >= n_moves)
         {
             printf("SEQUENCE DONE!\r\n");
             cmd->set_and(1,cmd->face);
@@ -216,7 +225,44 @@
             cmd->seq = NULL;
             return NULL;
         }
+        
+        // don't check if we're on the second to last move
+        int current_move = move_counts[cmd->face];
+        int second_to_last_move = n_moves - 1;
 
+
+        if( !(current_move >= second_to_last_move))
+        {
+            
+            // check the faces...
+            int face_1 =  cmd->seq->moves[move_counts[cmd->face]].face;
+            int face_2 =  cmd->seq->moves[move_counts[cmd->face] + 1].face;
+            
+            // if they don't match, go to old thing.
+            if(!check_double_move(face_1,face_2)) goto old_thing;
+            
+            // first check the current move...
+            if(cmd->face == face_1)
+                n_turns = cmd->seq->moves[move_counts[cmd->face]].n_turns;
+            else if(cmd->face == face_2)
+                n_turns = cmd->seq->moves[move_counts[cmd->face]+1].n_turns;
+            
+            move_counts[cmd->face]+=2;
+            
+            // rotate if needed
+            if(n_turns) 
+                cmd->rotate(n_turns,cmd->face);
+    
+            // wait...
+            states[cmd->face] = 1;
+            wait_counter[cmd->face] = 0;
+            return NULL;
+            
+        }
+
+old_thing:
+//***************** do the old thing ***************************
+        
         // check how many turns needed
         if(cmd->face == cmd->seq->moves[move_counts[cmd->face]].face)
         {
@@ -235,6 +281,7 @@
         states[cmd->face] = 1;
         wait_counter[cmd->face] = 0;
         return NULL;
+
     }
     else if(states[cmd->face] == 1)
     {

--- a/main.cpp	Sun Feb 04 04:37:05 2018 +0000
+++ b/main.cpp	Wed Mar 07 19:25:49 2018 +0000
@@ -9,12 +9,12 @@
 #define J               0.000065f            // Inertia
 #define KT              0.0678f              // Torque Constant
 #define R               0.85f               // Resistance
-#define V_IN            30.0f               // DC input voltage
+#define V_IN            60.0f               // DC input voltage
 #define K_SAT           22000.0f               // Controller saturation gain
 #define DTC_MAX         0.97f               // Max duty cycle (limited by bootstrapping)
 #define V               V_IN*DTC_MAX        // Max useable voltage
 
-#define TICKSTORAD(x)           (float)x*2.0f*PI/CPR  
+#define TICKSTORAD(x)           (float)x*2.0f*PI/CPR   
 #define CONSTRAIN(x,min,max)    ((x)<(min)?(min):((x)>(max)?(max):(x)))
 
 Serial pc (PA_2, PA_3);                     // Serial to programming header
@@ -23,7 +23,7 @@
 DigitalIn id_2(PB_4);
 DigitalIn id_1(PB_5);
 DigitalOut led(PA_15);                      // Debug LED
-DigitalIn d_in(PA_4);                       // LED on input from AND Board
+DigitalIn d_in(PA_4);                       // Input from AND Board
 
 //AnalogOut a_out(PA_5);
 DigitalOut d_out(PA_5);                     // LED on output to AND Board
@@ -47,6 +47,9 @@
 int count, count2;
 int controlmode = 0;
 
+float i_est;
+float i_int;
+
 
 volatile int run_control = 0;
 volatile int position_setpoint = 0;
@@ -143,7 +146,7 @@
     {
         ;
     }
-    wait(0.001f);
+    wait(0.0001f);
     printf("done.\r\n");
 }
 
@@ -156,6 +159,7 @@
 int8_t get_and_board()
 {
     uint8_t value = d_in;
+    led = value;
     //printf("[BOARD %d] Check and board: %d\r\n",get_board_id(),value);
     return value;
 }
@@ -302,6 +306,8 @@
     u = CONSTRAIN(e, -V, V);
     WriteVoltage(u);
     U = KT*(u - KT*dq)/R;
+    i_est = U;
+    i_int += U
     //WriteVoltage(-10.0f);
     }