Diff: main.cpp

Revision:

6:1143996ac690

Parent:

5:96cd67bcac8c

Child:

7:a278f58cdbd3

Child:

8:25a28a2ac486

--- a/main.cpp	Sun Jan 14 23:18:09 2018 +0000
+++ b/main.cpp	Mon Jan 15 16:12:52 2018 +0000
@@ -1,4 +1,6 @@
 #include "mbed.h"
+#include "cube.h"
+#include "communication.h"
 
 #define PI              3.14159265f
 #define PWM_ARR         0x2E8               // PWM timer auto-reload 
@@ -17,11 +19,13 @@
 
 Serial pc (PA_2, PA_3);                     // Serial to programming header
 Serial io(PB_6, PB_7);                      // Differential Serial to JST Header
-DigitalIn id_1(PB_3);                       // ID Setting Jumpers
+DigitalIn id_3(PB_3);                       // ID Setting Jumpers
 DigitalIn id_2(PB_4);
-DigitalIn id_3(PB_5);
+DigitalIn id_1(PB_5);
 DigitalOut led(PA_15);                      // Debug LED
 DigitalIn d_in(PA_4);                       // LED on input from AND Board
+
+//AnalogOut a_out(PA_5);
 DigitalOut d_out(PA_5);                     // LED on output to AND Board
 
 
@@ -30,7 +34,8 @@
 void InitPWM();
 void InitGPIO();
 void WriteVoltage( float v);
-int GetID();
+int ReadID();
+
 void SerialISR();
 
 
@@ -40,19 +45,23 @@
 int q_raw, dir, dq_raw = 0;
 float q, q_old, dq, u, e, q_ref, dqdebug = 0;
 int count, count2;
-int controlmode =0;
+int controlmode = 0;
+
+
+volatile int run_control = 0;
+volatile int position_setpoint = 0;
 
 /* Kalman Filter Variables */
 float q_est[2] = {0.0f};
 float q_meas[2] = {0.0f};
-float F[2][2] = {{1.0f, DT},{0.0f, 1.0f}};
-float B[2] = {0.0f, DT/J};
-float P[2][2] = {0};
-float Q[2] = {1.0f, 0.01f};
-float Rk[2] = {0.01, 10};
-float S[2][2] = {0};
-float Y[2] = {0};
-float K[2][2] = {0};
+//float F[2][2] = {{1.0f, DT},{0.0f, 1.0f}};
+//float B[2] = {0.0f, DT/J};
+//float P[2][2] = {0};
+//float Q[2] = {1.0f, 0.01f};
+//float Rk[2] = {0.01, 10};
+//float S[2][2] = {0};
+//float Y[2] = {0};
+//float K[2][2] = {0};
 float U;
 
 
@@ -65,9 +74,8 @@
   if (TIM1->SR & TIM_SR_UIF ) {
       }
       count++;
-    
     //d_out = !d_out;
-    
+    /*
     if(count>1000 && count<2000){
         q_ref = 1.57f;
         //ref = 18000.0f;
@@ -79,13 +87,13 @@
         //count = 0;
         }
     if(count>3000 && count<4000){
-        q_ref = -1.57f;
+        q_ref = 1.57f;
         }
         
     if(count>4500){
         controlmode = 1;
     }
-    /*
+    
     if(count<5000){
             //log_vec_2[count/4] = (int)(q_est[1]*10.0f);
             log_vec[count/4] = q_raw>>4;
@@ -99,6 +107,7 @@
         count2++;
         }
         
+        
         */
       Control();
       
@@ -112,29 +121,96 @@
           count = 0;
           }
           */
+          //a_out.write(q/2.0f);
           
   TIM1->SR = 0x0; // reset the status register
 }
 
+// FUNCTIONS TO MODIFY
+int get_board_id()
+{
+    return id;
+}
+
+void do_rotation(int8_t num_turns, int8_t derp)
+{
+    printf("[BOARD %d] Rotate %d turns!\r\n",get_board_id(),num_turns);   
+    position_setpoint += num_turns;
+    q_ref = 0.5f*PI*position_setpoint;
+    run_control = 1;
+    while(run_control)
+    {
+        ;
+    }
+    wait(0.5f);
+    printf("done.\r\n");
+}
+
+void set_and_board(int8_t value, int8_t derp)
+{
+    printf("[BOARD %d] Set and board %d\r\n",get_board_id(),value);
+    d_out = value;
+}
+
+int8_t get_and_board()
+{
+    uint8_t value = d_in;
+    //printf("[BOARD %d] Check and board: %d\r\n",get_board_id(),value);
+    return value;
+}
+
+
+
+// BEGIN STATE MACHINE CODE
+mbed_info_t state_machine_info;
+
+void state_machine_init()
+{
+    printf("Start state machine!\r\n");
+    int board_id = get_board_id();
+    
+    // set up state_machine_info
+    state_machine_info.face = (face_t)board_id;
+    pc.printf("\tboard id: %d\n",board_id);
+    // set null sequence to disable everything
+    state_machine_info.seq = NULL;
+    state_machine_info.rotate = do_rotation;
+    state_machine_info.set_and = set_and_board;
+    state_machine_info.get_and = get_and_board;
+    // prepare for serial
+    clear_message_buffer();
+}
+
+void handle_serial(Serial* pc)
+{
+    while(pc->readable())
+    {
+        // read it
+        uint8_t data = pc->getc();
+        if(data == '\n')
+        receive_move_sequence(pc,&state_machine_info);
+    }
+}
+
+// END STATE MACHINE CODE
 
 /* Main Loop */
 int main() {
 
     pc.baud(921600);
-    io.baud(921600);
+    io.baud(115200);
 
-    SystemCoreClockUpdate();
-    //printf("%d\n\r", SystemCoreClock);
-    //pc.printf("\n\r Rubix Controller\n\r");
+    pc.printf("\n\r Rubix Controller\n\r");
+    print_sample_sequence_hex();
     id_1.mode(PullUp);
     id_2.mode(PullUp);
     id_3.mode(PullUp);
-    id = GetID();
-    //pc.printf(" Motor ID:  %d\n\r", id);
+    id = ReadID();
+    pc.printf(" Motor ID:  %d\n\r", id);
     
     //d_in.mode(PullDown);
     led = 1;
-    d_out = 1;
+    d_out = 0;
     //wait(.1);
     
     InitPWM();
@@ -142,8 +218,14 @@
     //pc.printf("Initializing Encoder\n\r");
     //pc.printf("Initializing PWM\n\r");
     //wait(.1);
-    io.attach(&SerialISR);
+    //io.attach(&SerialISR);
+    
+    reset_mbed(); //MUST call this first thing in main - initializes data structures!
+    state_machine_init();
     while(1) {
+        //myled = get_board_id();
+        handle_serial(&io);
+        run_sequence_2(&state_machine_info); // run state machine
     }
 }
 
@@ -201,16 +283,21 @@
    
     
     // Control Law //
-    if(controlmode == 0){
+    if(run_control == 1){
        e = K_SAT*((q_ref - q) + (abs(dq)*dq*1.3f*R*J)/(2.0f*KT*(-V - KT*abs(dq))));   // Bullshit sliding mode control with nonlinear sliding surface, for minimum-time response
         //e = K_SAT*((q_ref - q) + (abs(q_est[1])*q_est[1]*1.3f*R*J)/(2.0f*KT*(-V - 1.0f*KT*abs(q_est[1]))));   // Bullshit sliding mode control with nonlinear sliding surface, for minimum-time response
 
     }
+    
     //q_ref = 0.0f;
-    if(controlmode == 1){
+    if(run_control == 0){
         e = 0;
         //e = 40.0f*(q_ref - q) + .2f*(0.0f-dq);
         }
+    if(run_control&(abs(q_ref - q))<.05f){
+        printf("control done\n\r");}
+    run_control = (abs(q_ref - q))>.05f;
+    
     u = CONSTRAIN(e, -V, V);
     WriteVoltage(u);
     U = KT*(u - KT*dq)/R;
@@ -237,13 +324,14 @@
     
     
 /* Read ID Jumpers */
-int GetID(void){
+int ReadID(void){
     int i1 = !id_1.read();
     int i2 = !id_2.read();
     int i3 = !id_3.read();
     return (i1<<2) | (i2<<1) | i3;
     }
 
+
 /* Initialize Encoder */
 void InitEncoder(void) {
     // configure GPIO PA0 & PA1 as inputs for Encoder