UVic Assistive Technology Lab / Mbed 2 deprecated DSLR_Camera_Gimbal

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed ros_lib_kinetic

lift.cpp@1:1ac7d472cfa2, 2018-02-06 (annotated)

Committer:
MikeGray92
Date:
Tue Feb 06 20:50:55 2018 +0000
Revision:
1:1ac7d472cfa2
Parent:
0:3a767f41cf04
Child:
2:0537a8007a39
Set up with external interrupt for lift. NOT TESTED

Who changed what in which revision?

UserRevisionLine numberNew contents of line
group-UVic-Assistive-Technolog 0:3a767f41cf04 1
group-UVic-Assistive-Technolog 0:3a767f41cf04 2 #include <stdint.h>
group-UVic-Assistive-Technolog 0:3a767f41cf04 3 #include "mbed.h"
group-UVic-Assistive-Technolog 0:3a767f41cf04 4 #include <ros.h>
group-UVic-Assistive-Technolog 0:3a767f41cf04 5 #include <std_msgs/Empty.h>
group-UVic-Assistive-Technolog 0:3a767f41cf04 6 #include <std_msgs/Float32MultiArray.h>
group-UVic-Assistive-Technolog 0:3a767f41cf04 7 #include <BNO055.h>
group-UVic-Assistive-Technolog 0:3a767f41cf04 8 #include <initializations.h>
group-UVic-Assistive-Technolog 0:3a767f41cf04 9 #include <definitions.h>
group-UVic-Assistive-Technolog 0:3a767f41cf04 10 #include <prototypes.h>
group-UVic-Assistive-Technolog 0:3a767f41cf04 11 #include <Mx28.h>
group-UVic-Assistive-Technolog 0:3a767f41cf04 12
group-UVic-Assistive-Technolog 0:3a767f41cf04 13 void runLift(void){
group-UVic-Assistive-Technolog 0:3a767f41cf04 14 if (liftFlag && control.liftRun){
group-UVic-Assistive-Technolog 0:3a767f41cf04 15 liftFlag = 0;
group-UVic-Assistive-Technolog 0:3a767f41cf04 16 // Set direction
group-UVic-Assistive-Technolog 0:3a767f41cf04 17 if(control.height > currentPosition){
group-UVic-Assistive-Technolog 0:3a767f41cf04 18 liftDirection.write(LIFTUP);
group-UVic-Assistive-Technolog 0:3a767f41cf04 19 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 20 else if(control.height < currentPosition){
group-UVic-Assistive-Technolog 0:3a767f41cf04 21 liftDirection.write(LIFTDOWN);
group-UVic-Assistive-Technolog 0:3a767f41cf04 22 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 23 // Check if arrived at destination height.
MikeGray92 1:1ac7d472cfa2 24 if (abs(control.height - currentPosition) < 5){
group-UVic-Assistive-Technolog 0:3a767f41cf04 25 control.liftRun = FALSE;
group-UVic-Assistive-Technolog 0:3a767f41cf04 26 liftSpeed.write(0);
group-UVic-Assistive-Technolog 0:3a767f41cf04 27 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 28 else {
group-UVic-Assistive-Technolog 0:3a767f41cf04 29 liftSpeed.write(0.5);
group-UVic-Assistive-Technolog 0:3a767f41cf04 30 // Check for stalling
group-UVic-Assistive-Technolog 0:3a767f41cf04 31 if(stall && liftSpeed.read() > 0){
MikeGray92 1:1ac7d472cfa2 32 stall = false;
MikeGray92 1:1ac7d472cfa2 33 control.liftRun = false;
group-UVic-Assistive-Technolog 0:3a767f41cf04 34 liftSpeed.write(0);
group-UVic-Assistive-Technolog 0:3a767f41cf04 35 if (liftDirection.read() == LIFTUP){
group-UVic-Assistive-Technolog 0:3a767f41cf04 36 currentPosition = LIFTHEIGHTMAX;
group-UVic-Assistive-Technolog 0:3a767f41cf04 37 }else{
group-UVic-Assistive-Technolog 0:3a767f41cf04 38 currentPosition = 0;
group-UVic-Assistive-Technolog 0:3a767f41cf04 39 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 40 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 41 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 42 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 43 }
group-UVic-Assistive-Technolog 0:3a767f41cf04 44
MikeGray92 1:1ac7d472cfa2 45 //// Keeps track of the rotation of the lift motor. After one full rotation up,
MikeGray92 1:1ac7d472cfa2 46 //// increments *position. If the motor doesn't rotate within
MikeGray92 1:1ac7d472cfa2 47 //// ((STALLTIME * HALLCHECKTIME * 4)/10^6) seconds, will set *stall TRUE.
MikeGray92 1:1ac7d472cfa2 48 //void checkLift(int& position, bool& stall) {
MikeGray92 1:1ac7d472cfa2 49 //
MikeGray92 1:1ac7d472cfa2 50 // static char state = 0;
MikeGray92 1:1ac7d472cfa2 51 // static char hallState1 = 0;
MikeGray92 1:1ac7d472cfa2 52 // static char hallState2 = 0;
MikeGray92 1:1ac7d472cfa2 53 // static char hallState1count = 0;
MikeGray92 1:1ac7d472cfa2 54 // static char hallState2count = 0;
MikeGray92 1:1ac7d472cfa2 55 // static char debounce = 0;
MikeGray92 1:1ac7d472cfa2 56 // static int stallcount = 0;
MikeGray92 1:1ac7d472cfa2 57 // int movementStart = position;
MikeGray92 1:1ac7d472cfa2 58 //
MikeGray92 1:1ac7d472cfa2 59 // // Debounce the hall sensors. Check them 3 times, if they are
MikeGray92 1:1ac7d472cfa2 60 // // high for 2 or more, set them as high, else low.
MikeGray92 1:1ac7d472cfa2 61 // if (debounce< 3){
MikeGray92 1:1ac7d472cfa2 62 // if (hallSensor1){
MikeGray92 1:1ac7d472cfa2 63 // hallState1count++;
MikeGray92 1:1ac7d472cfa2 64 // }
MikeGray92 1:1ac7d472cfa2 65 // if (hallSensor2){
MikeGray92 1:1ac7d472cfa2 66 // hallState2count++;
MikeGray92 1:1ac7d472cfa2 67 // }
MikeGray92 1:1ac7d472cfa2 68 // debounce++;
MikeGray92 1:1ac7d472cfa2 69 // }else{
MikeGray92 1:1ac7d472cfa2 70 // if (hallState1count > 1){
MikeGray92 1:1ac7d472cfa2 71 // hallState1 = 1;
MikeGray92 1:1ac7d472cfa2 72 // }else{
MikeGray92 1:1ac7d472cfa2 73 // hallState1 = 0;
MikeGray92 1:1ac7d472cfa2 74 // }
MikeGray92 1:1ac7d472cfa2 75 // if (hallState2count > 1){
MikeGray92 1:1ac7d472cfa2 76 // hallState2 = 1;
MikeGray92 1:1ac7d472cfa2 77 // }else{
MikeGray92 1:1ac7d472cfa2 78 // hallState2 = 0;
MikeGray92 1:1ac7d472cfa2 79 // }
MikeGray92 1:1ac7d472cfa2 80 // hallState1count = 0;
MikeGray92 1:1ac7d472cfa2 81 // hallState2count = 0;
MikeGray92 1:1ac7d472cfa2 82 //
MikeGray92 1:1ac7d472cfa2 83 // switch (state){
MikeGray92 1:1ac7d472cfa2 84 //
MikeGray92 1:1ac7d472cfa2 85 // case 0: // Hall 1 - low, Hall 2 - low
MikeGray92 1:1ac7d472cfa2 86 // if (hallState2 == 1){
MikeGray92 1:1ac7d472cfa2 87 // state = 1;
MikeGray92 1:1ac7d472cfa2 88 // position++;
MikeGray92 1:1ac7d472cfa2 89 // } else if (hallState1 == 1){
MikeGray92 1:1ac7d472cfa2 90 // state = 3;
MikeGray92 1:1ac7d472cfa2 91 // }
MikeGray92 1:1ac7d472cfa2 92 // break;
MikeGray92 1:1ac7d472cfa2 93 //
MikeGray92 1:1ac7d472cfa2 94 // case 1: // Hall 1 - low, Hall 2 - high
MikeGray92 1:1ac7d472cfa2 95 // if (hallState1 == 1){
MikeGray92 1:1ac7d472cfa2 96 // state = 2;
MikeGray92 1:1ac7d472cfa2 97 // } else if (hallState2 == 0){
MikeGray92 1:1ac7d472cfa2 98 // state = 0;
MikeGray92 1:1ac7d472cfa2 99 // position--;
MikeGray92 1:1ac7d472cfa2 100 // }
MikeGray92 1:1ac7d472cfa2 101 // break;
MikeGray92 1:1ac7d472cfa2 102 //
MikeGray92 1:1ac7d472cfa2 103 // case 2: // Hall 1 - high, Hall 2 - high
MikeGray92 1:1ac7d472cfa2 104 // if (hallState2 == 0){
MikeGray92 1:1ac7d472cfa2 105 // state = 3;
MikeGray92 1:1ac7d472cfa2 106 // } else if (hallState1 == 0){
MikeGray92 1:1ac7d472cfa2 107 // state = 1;
MikeGray92 1:1ac7d472cfa2 108 // }
MikeGray92 1:1ac7d472cfa2 109 // break;
MikeGray92 1:1ac7d472cfa2 110 //
MikeGray92 1:1ac7d472cfa2 111 // case 3: // Hall 1 - high, Hall 2 - low
MikeGray92 1:1ac7d472cfa2 112 // if (hallState1 == 0){
MikeGray92 1:1ac7d472cfa2 113 // state = 0;
MikeGray92 1:1ac7d472cfa2 114 // } else if (hallState2 == 1){
MikeGray92 1:1ac7d472cfa2 115 // state = 2;
MikeGray92 1:1ac7d472cfa2 116 // }
MikeGray92 1:1ac7d472cfa2 117 // break;
MikeGray92 1:1ac7d472cfa2 118 // }
MikeGray92 1:1ac7d472cfa2 119 // debounce = 0;
MikeGray92 1:1ac7d472cfa2 120 //
MikeGray92 1:1ac7d472cfa2 121 // // Check for stalling
MikeGray92 1:1ac7d472cfa2 122 // if (movementStart == position){
MikeGray92 1:1ac7d472cfa2 123 // stallcount++;
MikeGray92 1:1ac7d472cfa2 124 // if (stallcount >= STALLTIME){
MikeGray92 1:1ac7d472cfa2 125 // stall = TRUE;
MikeGray92 1:1ac7d472cfa2 126 // }
MikeGray92 1:1ac7d472cfa2 127 // } else{
MikeGray92 1:1ac7d472cfa2 128 // stallcount = 0;
MikeGray92 1:1ac7d472cfa2 129 // stall = FALSE;
MikeGray92 1:1ac7d472cfa2 130 // }
MikeGray92 1:1ac7d472cfa2 131 //
MikeGray92 1:1ac7d472cfa2 132 // }
MikeGray92 1:1ac7d472cfa2 133 //}