lift.cpp

00001  
00002 #include <stdint.h>
00003 #include "mbed.h"
00004 #include <ros.h>
00005 #include <std_msgs/Empty.h>
00006 #include <std_msgs/Int32MultiArray.h>
00007 #include <BNO055.h>
00008 #include <initializations.h>
00009 #include <definitions.h>
00010 #include <prototypes.h>
00011 #include <Mx28.h>
00012 
00013 bool startFlag = 0;
00014 
00015 void runLift(void){
00016     stallCheck();
00017     //Check If Arrived
00018     if (abs(control.height - currentPosition) < 6){ 
00019         liftSpeed.write(0);
00020         stopHallInt();
00021         startFlag = 0;
00022     }
00023     else {
00024 
00025         if(startFlag == 0){
00026             startHallInt();
00027             startFlag = 1;
00028         }   
00029         // Set direction
00030         if(control.height > currentPosition){
00031             liftDirection.write(LIFTUP);   
00032         }
00033         else if(control.height < currentPosition){
00034             liftDirection.write(LIFTDOWN);
00035         }         
00036        // liftSpeed.write(liftDutyCycle);
00037        liftSpeed.write(0.4);
00038     }  
00039 }
00040 
00041 //Determining if the lift stalled and stopping if it is
00042 void stallCheck(void){
00043     if(stallcount > 3 && liftSpeed.read() > 0 && eStopFlag == 0){
00044         liftSpeed.write(0);
00045         if (liftDirection.read() == LIFTUP){
00046             currentPosition = LIFTHEIGHTMAX;
00047             control.height = LIFTHEIGHTMAX;
00048         }else{
00049             currentPosition = 0;
00050             control.height = 0;
00051         }
00052     } 
00053 }