File content as of revision 1:1ac7d472cfa2:

#include <stdint.h>
#include "mbed.h"
#include <ros.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Float32MultiArray.h>
#include <BNO055.h>
#include <initializations.h>
#include <definitions.h>
#include <prototypes.h>
#include <Mx28.h>

void runLift(void){
    if (liftFlag && control.liftRun){
        liftFlag = 0;
        // Set direction
        if(control.height > currentPosition){
            liftDirection.write(LIFTUP);    
        }
        else if(control.height < currentPosition){
            liftDirection.write(LIFTDOWN);
        }
        // Check if arrived at destination height.
        if (abs(control.height - currentPosition) < 5){
            control.liftRun = FALSE;
            liftSpeed.write(0);
        }
        else {
            liftSpeed.write(0.5);
            // Check for stalling
            if(stall && liftSpeed.read() > 0){
                stall = false;
                control.liftRun = false;
                liftSpeed.write(0);
                if (liftDirection.read() == LIFTUP){
                    currentPosition = LIFTHEIGHTMAX;
                }else{
                    currentPosition = 0;
                }
            }
        }       
    }    
}

//// Keeps track of the rotation of the lift motor.  After one full rotation up,
//// increments *position. If the motor doesn't rotate within 
//// ((STALLTIME * HALLCHECKTIME * 4)/10^6) seconds, will set *stall TRUE. 
//void checkLift(int& position, bool& stall) {
//    
//    static char state = 0;
//    static char hallState1 = 0;
//    static char hallState2 = 0;
//    static char hallState1count = 0;
//    static char hallState2count = 0;
//    static char debounce = 0;
//    static int  stallcount = 0;
//    int movementStart = position;
//    
//    // Debounce the hall sensors. Check them 3 times, if they are
//    // high for 2 or more, set them as high, else low.
//    if (debounce< 3){
//        if (hallSensor1){
//            hallState1count++;
//        }
//        if (hallSensor2){
//            hallState2count++;
//        }
//        debounce++;
//    }else{
//        if (hallState1count > 1){
//            hallState1 = 1;
//        }else{
//            hallState1 = 0;
//        }
//        if (hallState2count > 1){
//            hallState2 = 1;
//        }else{
//            hallState2 = 0;
//        }
//        hallState1count = 0;
//        hallState2count = 0;       
//        
//        switch (state){
//            
//            case 0: // Hall 1 - low, Hall 2 - low
//                    if (hallState2 == 1){
//                        state = 1;
//                        position++;
//                    } else if (hallState1 == 1){
//                        state = 3;
//                    }
//                    break;
//                    
//            case 1: // Hall 1 - low, Hall 2 - high
//                    if (hallState1 == 1){
//                        state = 2;
//                    } else if (hallState2 == 0){
//                        state = 0;
//                        position--;
//                    }
//                    break;
//                    
//            case 2: // Hall 1 - high, Hall 2 - high
//                    if (hallState2 == 0){
//                        state = 3;
//                    } else if (hallState1 == 0){
//                        state = 1;
//                    }
//                    break;
//                    
//            case 3: // Hall 1 - high, Hall 2 - low
//                    if (hallState1 == 0){
//                        state = 0;
//                    } else if (hallState2 == 1){
//                        state = 2;
//                    }
//                    break;
//        }
//        debounce = 0;
//        
//        // Check for stalling
//        if (movementStart == position){
//            stallcount++;
//            if (stallcount >= STALLTIME){
//                stall = TRUE;
//            }
//        } else{
//            stallcount = 0;
//            stall = FALSE;
//        }
//
//    }
//}