Revision 5:498c9bfc56f0, committed 2015-02-25

Comitter:

cashdollar

Date:

Wed Feb 25 19:23:41 2015 +0000

Parent:

4:f4210aefaa65

Child:

6:ff3677c61389

Commit message:

Added safety checks. Safety checks include: 1. both encoders: range, parity, flag 2.both knees: status 3. IMU whoamI. Safety is checked every control cycle and send to dataBed.

Changed in this revision

--- a/FiniteStateMachine/FSM.cpp	Fri Feb 13 23:02:24 2015 +0000
+++ b/FiniteStateMachine/FSM.cpp	Wed Feb 25 19:23:41 2015 +0000
@@ -12,6 +12,7 @@
 
 SPI dummy(p5,p6,p7);
 DigitalOut cs_dummy(p15);
+//DigitalOut test(p7);
 SPI imuspi(p11, p12, p13);
 mpu6000_spi imu(imuspi,p20);
 // FSM Variables
@@ -23,22 +24,22 @@
 /*TO DO:
     Add initial pose recognition and blend
     -Implement backBias without going over memory- check 11/10, I think this is taken care of by the zero_ang
-    Add encoder safety checks
+    - Add encoder safety checks - complete 2/25/2015
     -Change to knee variables- check 11/17/14
-    Add knee status check
-    Fix the press and hold pointer function
+    - knee status check- complete 2/25/2015
+    - Fix the press and hold pointer function
     -Change linear blend- check 11/11
     -Implement new UI command routine-check 10/15
     -Design new databed routine- check 10/15, needs improvement, they are out of sync if not started together
-    Implement new motorMusic routine
+    -Implement new motorMusic routine
     -Implement press and hold- check 11/10
     -Try pointers to the trajectories.  Keep running out of memory- check 11/11
-    Create abstract class for the states, e.g. step, FS, FTG
-    Change backBias to the global one
-    Change knee init, there's one in the constructor
-    Remove rollover detection in FSM constructor
-    Add LEDs for debugging
-    Look into adding clear to fix bend2Sit drop problem
+    -Create abstract class for the states, e.g. step, FS, FTG
+    -Change backBias to the global one
+    -Change knee init, there's one in the constructor
+    -Remove rollover detection in FSM constructor
+    -Add LEDs for debugging
+    -Look into adding clear to fix bend2Sit drop problem
 */
 FSM::FSM():exoState(DoubleStance),PH_UI(1), side(1), nextState(0), currentState(0), timeout(2), t_wait(2), count(0), count_max(1000),
     ptrTraj_L(ref_swing_step), ptrTraj_R(ref_stance_step),t_blend(100),pos_L(encoder_L.angle()), pos_R(encoder_R.angle()),backBias(6), kneeThresh(500),music(hip_L,hip_R), prevUI(0), torso(0)
@@ -110,57 +111,71 @@
     hip_R.off();*/
 }
 
-/***********************************************************
-Function:         FSM::error()
-Access Specifier: Public
-Use:              Safety check. Gathers errors from encoder, IMU, and knee stepper.
-Input:            From encoder:
-                  enc_safe_range - from Encoder. Low = safe, high = unsafe.
-                  enc_parity - Low = unsafe, high = safe.
-                  enc_flag - Low = safe, high = unsafe.
-                  From IMU:
-                  whoami - Low = safe, high = unsafe.
-                  From knee stepper:
-                  knee_error - Low = safe, high = unsafe.
-Output:           error. Low = safe, high = unsafe.
-***********************************************************/
-int FSM::error(bool encoder_L.parityFlag(), bool encoder_L.encFlag(), bool encoder_L.rangeCheck(), bool encoder_R.parityFlag(), bool encoder_R.encFlag(), bool encoder_R.rangeCheck(), int imu.whoami_check(), int kneeflags)
+
+short int FSM::error()
 {
-    // create array for storing all safety flags
-    int safety_flag[8];
+    // Initialize error_array with all 0 (safe)
+    bool error_array[16]= {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    
+    // Fill error flag array with collected errors from all programs
+    // In all elements, 0 is safe, 1 is unsafe
+    error_array[0] = !(encoder_L.parityFlag()|1); 
+    error_array[1] = encoder_L.encFlag(); 
+    error_array[2] = encoder_L.rangeCheck();
+    error_array[3] = !(encoder_R.parityFlag()|1); 
+    error_array[4] = encoder_R.encFlag();
+    error_array[5] = encoder_R.rangeCheck(); 
+    pc.printf("IMU: %x, ", imu.whoami());
+    error_array[6] = imu.whoami_check(); 
+    
+    // Toggle communcation - Fix this later
+    cs_dummy=0;
+    dummy.write(0);
+    cs_dummy=1;
     
-    // fill safety_flag array
-    safety_flag[1] = !(encoder_L.parityFlag()|1); // opposite of high is safe
-    safety_flag[2] = (encoder_L.encFlag()|0); // low if safe
-    safety_flag[3] = (encoder_L.rangeCheck()|0); // low is safe
-    safety_flag[4] = !(encoder_R.parityFlag()|1); // opposite of high is safe
-    safety_flag[5] = (encoder_R.encFlag()|0); // low is safe
-    safety_flag[6] = (encoder_R.rangeCheck()|0); // low is safe
-    safety_flag[7] = (imu.whoami_check()|0); // low is safe
-    safety_flag[8] =
-    /* first draft
-    _parity_L = !(encoder_L.parityFlag()|1); // opposite of high is safe
-    _enc_flag_L = (encoder_L.encFlag()|0); // low if safe
-    _range_L = (encoder_L.rangeCheck()|0); // low is safe
-    _parity_R = !(encoder_R.parityFlag()|1); // opposite of high is safe
-    _enc_flag_R = (encoder_R.encFlag()|0); // low is safe
-    _range_R = (encoder_R.rangeCheck()|0); // low is safe
-    */
-    _imu_who = 
-    
-    return
- }   
-    
+    // cCntinue to fill array
+    error_array[7] = (knee_L.knee_status()|0);
+    error_array[8] = (knee_R.knee_status()|0);
+
 
+    // Create error_byte from components of error_array
+    error_byte = 0;
+    for (int i = 0; i<15; ++i) {
+        if(error_array[i]) {
+            error_byte |= 1 <<i;
+        }
+    }
+    pc.printf("ERR: %x, ", error_byte);
+    return error_byte;
+}
+
+bool FSM::is_Safe()
+{
+    bool safety_flag = 0;
+    if (error_byte != 0x0000) {
+        safety_flag = 1;
+    }
+    return safety_flag;
+
+}
 
 int FSM::state(int UI)
 {
-    pc.printf("%d, ", exoState);
+    pc.printf("EX: %d, ", exoState);
+    
     pos_L=encoder_L.angle();
     pos_R=encoder_R.angle();
     torso=imu.angle_y();
+    
     pc.printf("%f, ", torso);
-      
+    
+    pc.printf("SAFE %d, ", is_Safe());
+    
+    // Check for safety. If unsafe, move to exoState = 0
+    if (is_Safe() == 1) {
+        exoState = 0;
+    }
+
     music.check();
     switch(exoState) {
         case Sitting:
@@ -384,7 +399,7 @@
         case 0:
             hip_L.off();
             hip_R.off();
-            pc.printf("%f, %f, %f, %f\r\n", pos_L, pos_R, 0, hip_L.read());
+            pc.printf("pos_L:%f, pos_R:%f, 0:%f, hip_L:%f, \r\n", pos_L, pos_R, 0, hip_L.read());
             break;
     }
     return exoState;
@@ -439,7 +454,7 @@
         cs_dummy=1;
         knee_L.lock();
         knee_R.lock();
-        pc.printf(", %x, %x", knee_L.status(), knee_R.status());
+        pc.printf(", knee_L:%x, knee_R:%x", knee_L.status(), knee_R.status());
     }
     pc.printf("\r\n");
     count++;

--- a/FiniteStateMachine/FSM.h	Fri Feb 13 23:02:24 2015 +0000
+++ b/FiniteStateMachine/FSM.h	Wed Feb 25 19:23:41 2015 +0000
@@ -29,7 +29,17 @@
     int currentState;
     int exoState;  // state of exo in FSM
     void init();
-
+    
+    /** Create instance of error()
+    * error() creates an array of all error checks and the concatenates them into 2 bytes
+    * @ returns
+    *    error_byte
+    *    error_byte = 0x0 is safe.
+    *    error_byte != 0x0 is unsafe.
+    */
+    short int error();
+    
+    
 private:
     //typedef void (FSM::*fooPtr)(void); //temporary function type
     typedef void (FSM::*fooPtr)(void); //temporary function type
@@ -38,6 +48,30 @@
     LinearBlend blend_L;
     LinearBlend blend_R;
     MotorMusic music;
+    
+    /** Create instance of error_byte
+    * error_byte is 2 bytes, concatenated elements of error_array
+    * error_byte = 0x0 is safe.
+    * error_byte != 0x0 is unsafe.
+    */
+    short int error_byte;
+    
+    /** Create instance of safety_flag
+    * safety_flag is an analysis of error_byte that checks for safety
+    * safety_flag = 0 is safe.
+    * safety_flag = 1 is unsafe.
+    */
+    bool safety_flag;
+    
+    /** Create instance of is_Safe()
+    * is_Safe() analyzes error_byte to check for safe conditions
+    * @ returns
+    *    safety_flag
+    *    safety_flag = 0 is safe.
+    *    safety_flag = 1 is unsafe.
+    */
+    bool is_Safe();
+    
     void stepInit();
     void step();
     void doubleStance();

--- a/Knee/StepperDriver/StepperDriver.cpp	Fri Feb 13 23:02:24 2015 +0000
+++ b/Knee/StepperDriver/StepperDriver.cpp	Wed Feb 25 19:23:41 2015 +0000
@@ -27,15 +27,16 @@
     return paramHandler(param,0);
 }
 
+
 int StepperDriver::getStatus()
 {
   int temp = 1;
   char* bytePointer = (char*)&temp;
-  // Write GetStatus command structure
   SPIWrite(GET_STATUS);
   bytePointer[1] = SPIWrite(0);
   bytePointer[0] = SPIWrite(0);
   return temp;
+
 }    
     
 void StepperDriver::run(char dir, float stepsPerSec)
@@ -126,7 +127,7 @@
 {
     float temp = stepsPerSec*67.106;
     if( (unsigned long) long(temp) > 0x000FFFFF) return 0x000FFFFF;
-    else return (unsigned long) temp;
+else return (unsigned long) temp;
 }
 
 unsigned long StepperDriver::maxSpdCalc(float stepsPerSec)
@@ -183,10 +184,18 @@
 
 char StepperDriver::SPIWrite(char data)
 {
+    // initialize rxData
     char rxData;
+    
+    //lower comm pin
     _CSPin = 0;
+    
+    //Get reading from the pin at the declared address(data)
     rxData = SPIPin.write(data);
+    
+    // raise comm pin
     _CSPin = 1;
+    
     return rxData;
 }
 

--- a/Knee/StepperDriver/StepperDriver.h	Fri Feb 13 23:02:24 2015 +0000
+++ b/Knee/StepperDriver/StepperDriver.h	Wed Feb 25 19:23:41 2015 +0000
@@ -35,6 +35,7 @@
     
     private:
     
+    int hey1;
     char SPIWrite (char data);
     unsigned long writeParam (unsigned long value, char bitLength);
     unsigned long paramHandler (char param, unsigned long value);
@@ -86,7 +87,7 @@
 #define HARD_STOP            0xB8
 #define SOFT_HIZ             0xA0
 #define HARD_HIZ             0xA8
-#define GET_STATUS           0xD0 // L6482 Manual page 69
+#define GET_STATUS           0xD0 // L6482 Manual page 59
 
 // Register address redefines.
 //  See the Param_Handler() function for more info about these.

--- a/Knee/knee.cpp	Fri Feb 13 23:02:24 2015 +0000
+++ b/Knee/knee.cpp	Wed Feb 25 19:23:41 2015 +0000
@@ -38,21 +38,46 @@
     stepper.setParam(DEC,0xFFE);
 }
 
-// Checks status of knee
+/***********************************************************
+Function:         Knee::stepper_status()
+Access Specifier: Public
+Use:              Public getter function. Allows access to status value via outside functions.
+                  Collects getStatus value from stepper driver.
+Input:            None
+Output:           stepper.getStatus() -
+***********************************************************/
 int Knee::status()
 {
+
     return stepper.getStatus();
 }
-// Evaluates status of knee for safety
-// Should this be combined with the above function?
-bool Knee::safe_status()
+
+/***********************************************************
+Function:         Knee::knee_status()
+Access Specifier: Public
+Use:              Public getter function. Allows access to status value via outside functions.
+                  Collects getStatus value from stepper driver.
+Input:            None
+Output:           knee_error. 0 is safe. 1 is unsafe.
+***********************************************************/
+int Knee::knee_status()
 {
-    // grab status
-    status = stepper.getStatus();
-    // Bitwise and to find if any safety issues exist
-    // High = unsafe
-    status = (status | 
-        return status;
+    // create instance of knee_error
+    //int knee_error;
+    // Assign status to knee_flag for further analysis
+    knee_flag = status();
+    
+    // Mask against minimum safety requirement
+    // Safety requirements gathered from L6482 Stepper Spec, page 58
+    int knee_test;
+    knee_test = (knee_flag & 0x2600);
+
+       // Confirm that safety requirements are met
+    // 0 is safe
+    //1 is unsafe
+    knee_error = !(knee_test == 0x2600);
+    
+    return knee_error;
 }
 
 void Knee::lock()
@@ -67,7 +92,8 @@
     stepper.move(REV, 0x0001A66);//6758 1/16th microsteps, minimum is 5120
 }
 
+
 void Knee::debug()
 {
-    pc2.printf("%x, %x, %x\r\n", stepper.getParam(CONFIG), stepper.getStatus(), stepper.getParam(TVAL_HOLD));
+    pc2.printf("KNEEDEBUG %x, %x, %x\r\n", stepper.getParam(CONFIG), stepper.getStatus(), stepper.getParam(TVAL_HOLD));
 }
\ No newline at end of file

--- a/Knee/knee.h	Fri Feb 13 23:02:24 2015 +0000
+++ b/Knee/knee.h	Wed Feb 25 19:23:41 2015 +0000
@@ -1,7 +1,8 @@
-/* L6482 Driver code communicated via SPI
- * file name: StepperDriver.h
- * 07/15/2014 edited by Grace
- */
+/** Knee class.
+* Used to control knee mechanism in conjunction with StepperDriver.h.
+* L6482 Stepper Driver code communicated via SPI.
+* 02/25/2015 Edited by Hayley Cashdollar
+*/
 
 #ifndef KNEE_H
 #define KNEE_H
@@ -18,8 +19,37 @@
     void lock();
     void unlock();
     void init();
+    
+    /** Create status instance
+    * status() gathers the current status from the knee stepper.
+    * @returns
+    *    2 byte status from Stepper. See page 58 of L6482 Stepper Spec for detail.
+    *
+    */
     int status();
+
+    /** Create knee_status instance.
+    * knee_status() compares stepper status to safe status.
+    * @ returns
+    *    0 for safe
+    *    1 for unsafe
+    */
+    int knee_status();
+
+    /** Create knee_error instance.
+    * knee_error is the output of knee_status.
+    * knee_error = 0 is safe
+    * knee_error = 1 is unsafe.
+    */
+    int knee_error;
+
+    /** Create knee_flag instance.
+    * knee_flag is an intermediate integer in computation of knee_status
+    */
+    int knee_flag;
+    
     void debug();
+
 private:
     StepperDriver stepper;
 

--- a/Sensors/Encoders.lib	Fri Feb 13 23:02:24 2015 +0000
+++ b/Sensors/Encoders.lib	Wed Feb 25 19:23:41 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/cashdollar/code/Encoders/#a5422fafc307
+http://developer.mbed.org/users/cashdollar/code/Encoders/#d043b7211b3c

--- a/Sensors/IMUdriver.lib	Fri Feb 13 23:02:24 2015 +0000
+++ b/Sensors/IMUdriver.lib	Wed Feb 25 19:23:41 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/cashdollar/code/IMUdriver/#1d985e2d60a6
+http://developer.mbed.org/users/cashdollar/code/IMUdriver/#b54fd8d53035

--- a/initExoVars/initExoVars.cpp	Fri Feb 13 23:02:24 2015 +0000
+++ b/initExoVars/initExoVars.cpp	Wed Feb 25 19:23:41 2015 +0000
@@ -84,6 +84,8 @@
     // Putting various comm pins high
     DB_cs = 1; // Setting SPI CS pin high (so low selects the slave)
     pc.baud(921600); //pc.baud(115200);//460800
+    
+    // initialize both knees
     knee_L.init();
     knee_R.init();
 //Setting pwm frequencies to 20kHz
@@ -94,13 +96,18 @@
     encoder_L.init(zero_ang_L);
     encoder_R.init(zero_ang_R);
     encoder_R.flip();
-    knee_L.debug();
-    knee_R.debug();
+    // display debugging info for both knees
+    //knee_L.debug();
+    //knee_R.debug();
     fsm.init();
-    encoder_L.ranges();
-    encoder_R.ranges();
+    encoder_L.ranges(_enc_low, _enc_high);
+    encoder_R.ranges(_enc_low, _enc_high);
+    encoder_L.rangeCheck();
+    encoder_R.rangeCheck();
 
     dataBedSPI.frequency(1000000);
-    pc.printf("Knee: %x, %x\r\n", knee_L.status(), knee_R.status());
+    
+    // print each knee status
+    pc.printf("knee_L: %x, knee_R:%x\r\n,", knee_L.status(), knee_R.status());
 
 }

--- a/main.cpp	Fri Feb 13 23:02:24 2015 +0000
+++ b/main.cpp	Wed Feb 25 19:23:41 2015 +0000
@@ -24,29 +24,29 @@
 void periodicFcns()
 {
     dataOut[1]=encoder_L.readRaw();
-    dataOut[2]=encoder_R.readRaw();
+    dataOut[2]=fsm.error();
     int* ptr=dataIn;
     ptr=sendData(dataOut, 4, dataIn);
-    // why only 3 of the %d?
-    pc.printf("%d, %d, %d, %d, ", dataIn[0], dataIn[1], dataIn[2], dataIn[3]);
-    
+       
 
+    pc.printf("%d, %d, %d, %d,", dataIn[0], dataIn[1], dataIn[2], dataIn[3]);
     
     // Run state change/analysis in FSM
     int exoState=fsm.state(dataIn[1]);
     
-
 }
 
 int main()
 {
-    pc.printf("ExoStart \r\n");
+    pc.printf("\r\nExoStart \r\n");
     initializeExoIOs();
-    // pc.printf("Blah\r\n");
+    pc.printf("Test\r\n"); // keep for debugging compile errors
 
     pc.printf("Starting exo...\n\r");
+
     //If desired, a startup sound can be played. This function is defined in the DatabedCode, because it will command a sound to be played once it detects a heartbeat from ControlBed
     wait(2);
+
     Ticker doControl;
     dataBedSPI.format(8,1);
     doControl.attach(&periodicFcns, SAMPLE_TIME);