Puppet Master System / Mbed 2 deprecated Glove_transmit_Coordinator

This code is put on the glove. It reads the data from the flex sensors and outputs the appropriate ASCII characters that tell how the robot should move.

Dependencies:   PinDetect mbed

main.cpp@0:3e074f9f4063, 2018-04-26 (annotated)

Committer:
fyousuf3
Date:
Thu Apr 26 15:07:05 2018 +0000
Revision:
0:3e074f9f4063
This looks at the Flex Sensors and outputs ASCII characters that relay what speed the robot should move

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fyousuf3 0:3e074f9f4063 1 #include "mbed.h"
fyousuf3 0:3e074f9f4063 2 #include "PinDetect.h"
fyousuf3 0:3e074f9f4063 3
fyousuf3 0:3e074f9f4063 4 //Analog input. Finger 1-5. Thumb is 1. Pinky is 5.
fyousuf3 0:3e074f9f4063 5 AnalogIn a1(p16);
fyousuf3 0:3e074f9f4063 6 AnalogIn a2(p17);
fyousuf3 0:3e074f9f4063 7 AnalogIn a3(p18);
fyousuf3 0:3e074f9f4063 8 AnalogIn a4(p19);
fyousuf3 0:3e074f9f4063 9 AnalogIn a5(p20);
fyousuf3 0:3e074f9f4063 10
fyousuf3 0:3e074f9f4063 11 //Coordinator Setup for XBEE
fyousuf3 0:3e074f9f4063 12 Serial xbee1(p9,p10);
fyousuf3 0:3e074f9f4063 13 DigitalOut rst1(p11);
fyousuf3 0:3e074f9f4063 14
fyousuf3 0:3e074f9f4063 15 DigitalOut myled1(LED1);
fyousuf3 0:3e074f9f4063 16 DigitalOut myled2(LED2);
fyousuf3 0:3e074f9f4063 17 DigitalOut myled3(LED2);
fyousuf3 0:3e074f9f4063 18 DigitalOut myled4(LED2);
fyousuf3 0:3e074f9f4063 19
fyousuf3 0:3e074f9f4063 20 // Global count variable
fyousuf3 0:3e074f9f4063 21 float volatile af1=0.5;
fyousuf3 0:3e074f9f4063 22 float volatile af2=0.5;
fyousuf3 0:3e074f9f4063 23 float volatile af3=0.5;
fyousuf3 0:3e074f9f4063 24 float volatile af4=0.5;
fyousuf3 0:3e074f9f4063 25 float volatile af5=0.5;
fyousuf3 0:3e074f9f4063 26
fyousuf3 0:3e074f9f4063 27 float volatile afs1=0.5;
fyousuf3 0:3e074f9f4063 28 float volatile afs2=0.5;
fyousuf3 0:3e074f9f4063 29 float volatile afs3=0.5;
fyousuf3 0:3e074f9f4063 30 float volatile afs4=0.5;
fyousuf3 0:3e074f9f4063 31 float volatile afs5=0.5;
fyousuf3 0:3e074f9f4063 32
fyousuf3 0:3e074f9f4063 33 Serial pc(USBTX, USBRX);
fyousuf3 0:3e074f9f4063 34
fyousuf3 0:3e074f9f4063 35 //calibration constants
fyousuf3 0:3e074f9f4063 36 float const c1_low = 0.524;
fyousuf3 0:3e074f9f4063 37 float const c2_low = 0.469;
fyousuf3 0:3e074f9f4063 38 float const c3_low = 0.477;
fyousuf3 0:3e074f9f4063 39 float const c4_low = 0.424;
fyousuf3 0:3e074f9f4063 40 float const c5_low = 0.471;
fyousuf3 0:3e074f9f4063 41
fyousuf3 0:3e074f9f4063 42 float const c1_high = 0.661;
fyousuf3 0:3e074f9f4063 43 float const c2_high = 0.642;
fyousuf3 0:3e074f9f4063 44 float const c3_high = 0.693;
fyousuf3 0:3e074f9f4063 45 float const c4_high = 0.690;
fyousuf3 0:3e074f9f4063 46 float const c5_high = 0.713;
fyousuf3 0:3e074f9f4063 47
fyousuf3 0:3e074f9f4063 48 //function prototypes:
fyousuf3 0:3e074f9f4063 49 float myConstrain(float val, float min, float max);
fyousuf3 0:3e074f9f4063 50 float myScale(float val, float min, float max);
fyousuf3 0:3e074f9f4063 51 float myConstrainScale(float val, float min, float max);
fyousuf3 0:3e074f9f4063 52
fyousuf3 0:3e074f9f4063 53 int main() {
fyousuf3 0:3e074f9f4063 54 const float dampingCoeff = 0.5; //tweak this
fyousuf3 0:3e074f9f4063 55 rst1 = 0; //Set reset pin to 0
fyousuf3 0:3e074f9f4063 56 myled1 = 0;
fyousuf3 0:3e074f9f4063 57 myled2 = 0;
fyousuf3 0:3e074f9f4063 58 myled3 = 0;
fyousuf3 0:3e074f9f4063 59 myled4 = 0;
fyousuf3 0:3e074f9f4063 60 wait_ms(1);
fyousuf3 0:3e074f9f4063 61 rst1 = 1; //Set reset pin to 1
fyousuf3 0:3e074f9f4063 62 wait_ms(1);
fyousuf3 0:3e074f9f4063 63
fyousuf3 0:3e074f9f4063 64 while(1) {
fyousuf3 0:3e074f9f4063 65 //filter:
fyousuf3 0:3e074f9f4063 66 af1 += dampingCoeff * ( a1 - af1 );
fyousuf3 0:3e074f9f4063 67 af2 += dampingCoeff * ( a2 - af2 );
fyousuf3 0:3e074f9f4063 68 af3 += dampingCoeff * ( a3 - af3 );
fyousuf3 0:3e074f9f4063 69 af4 += dampingCoeff * ( a4 - af4 );
fyousuf3 0:3e074f9f4063 70 af5 += dampingCoeff * ( a5 - af5 );
fyousuf3 0:3e074f9f4063 71
fyousuf3 0:3e074f9f4063 72 //constrain and scale:
fyousuf3 0:3e074f9f4063 73 afs1 = myConstrainScale(af1, c1_low, c1_high);
fyousuf3 0:3e074f9f4063 74 afs2 = myConstrainScale(af2, c2_low, c2_high);
fyousuf3 0:3e074f9f4063 75 afs3 = myConstrainScale(af3, c3_low, c3_high);
fyousuf3 0:3e074f9f4063 76 afs4 = myConstrainScale(af4, c4_low, c4_high);
fyousuf3 0:3e074f9f4063 77 afs5 = myConstrainScale(af5, c5_low, c5_high);
fyousuf3 0:3e074f9f4063 78
fyousuf3 0:3e074f9f4063 79
fyousuf3 0:3e074f9f4063 80 if (0.3 >= afs1 > 0.5) { // Thumb
fyousuf3 0:3e074f9f4063 81 xbee1.putc ('A'); // Move back speed 1
fyousuf3 0:3e074f9f4063 82
fyousuf3 0:3e074f9f4063 83 myled1 = 1;
fyousuf3 0:3e074f9f4063 84 myled2 = 0;
fyousuf3 0:3e074f9f4063 85 myled3 = 0;
fyousuf3 0:3e074f9f4063 86 myled4 = 0;
fyousuf3 0:3e074f9f4063 87 }
fyousuf3 0:3e074f9f4063 88 else if (afs1 >= 0.5) { // Thumb
fyousuf3 0:3e074f9f4063 89 xbee1.putc ('B'); // Move back speed 2
fyousuf3 0:3e074f9f4063 90
fyousuf3 0:3e074f9f4063 91 myled1 = 1;
fyousuf3 0:3e074f9f4063 92 myled2 = 0;
fyousuf3 0:3e074f9f4063 93 myled3 = 0;
fyousuf3 0:3e074f9f4063 94 myled4 = 0;
fyousuf3 0:3e074f9f4063 95 }
fyousuf3 0:3e074f9f4063 96 else if (0.3 >= afs2 > 0.5) { // Pointer Finger
fyousuf3 0:3e074f9f4063 97 xbee1.putc ('C'); // Move right speed 1
fyousuf3 0:3e074f9f4063 98
fyousuf3 0:3e074f9f4063 99 myled1 = 0;
fyousuf3 0:3e074f9f4063 100 myled2 = 1;
fyousuf3 0:3e074f9f4063 101 myled3 = 0;
fyousuf3 0:3e074f9f4063 102 myled4 = 0;
fyousuf3 0:3e074f9f4063 103 }
fyousuf3 0:3e074f9f4063 104 else if (afs2 >= 0.5) { // Pointer Finger
fyousuf3 0:3e074f9f4063 105 xbee1.putc ('D'); // Move right speed 2
fyousuf3 0:3e074f9f4063 106
fyousuf3 0:3e074f9f4063 107 myled1 = 0;
fyousuf3 0:3e074f9f4063 108 myled2 = 1;
fyousuf3 0:3e074f9f4063 109 myled3 = 0;
fyousuf3 0:3e074f9f4063 110 myled4 = 0;
fyousuf3 0:3e074f9f4063 111 }
fyousuf3 0:3e074f9f4063 112 else if (0.3 >= afs3 > 0.5) { // Middle Finger
fyousuf3 0:3e074f9f4063 113 xbee1.putc ('E'); // Move Forward speed 1
fyousuf3 0:3e074f9f4063 114
fyousuf3 0:3e074f9f4063 115 myled1 = 0;
fyousuf3 0:3e074f9f4063 116 myled2 = 0;
fyousuf3 0:3e074f9f4063 117 myled3 = 1;
fyousuf3 0:3e074f9f4063 118 myled4 = 0;
fyousuf3 0:3e074f9f4063 119 }
fyousuf3 0:3e074f9f4063 120 else if (afs3 >= 0.5) { // Middle Finger
fyousuf3 0:3e074f9f4063 121 xbee1.putc ('F'); // Move Forward speed 1
fyousuf3 0:3e074f9f4063 122
fyousuf3 0:3e074f9f4063 123 myled1 = 0;
fyousuf3 0:3e074f9f4063 124 myled2 = 0;
fyousuf3 0:3e074f9f4063 125 myled3 = 1;
fyousuf3 0:3e074f9f4063 126 myled4 = 0;
fyousuf3 0:3e074f9f4063 127 }
fyousuf3 0:3e074f9f4063 128 else if (0.3 >= afs4 > 0.5) { //Ring Finger
fyousuf3 0:3e074f9f4063 129 xbee1.putc ('G'); // Move Left speed 1
fyousuf3 0:3e074f9f4063 130
fyousuf3 0:3e074f9f4063 131 myled1 = 1;
fyousuf3 0:3e074f9f4063 132 myled2 = 0;
fyousuf3 0:3e074f9f4063 133 myled3 = 0;
fyousuf3 0:3e074f9f4063 134 myled4 = 0;
fyousuf3 0:3e074f9f4063 135 }
fyousuf3 0:3e074f9f4063 136 else if (afs4 >= 0.5) { // Ring Finger
fyousuf3 0:3e074f9f4063 137 xbee1.putc ('H'); // Move Left speed 2
fyousuf3 0:3e074f9f4063 138
fyousuf3 0:3e074f9f4063 139 myled1 = 1;
fyousuf3 0:3e074f9f4063 140 myled2 = 0;
fyousuf3 0:3e074f9f4063 141 myled3 = 0;
fyousuf3 0:3e074f9f4063 142 myled4 = 0;
fyousuf3 0:3e074f9f4063 143 }
fyousuf3 0:3e074f9f4063 144 else if (afs5 >= 0.5) { // Pinky
fyousuf3 0:3e074f9f4063 145 xbee1.putc ('I'); // Move back speed 2
fyousuf3 0:3e074f9f4063 146
fyousuf3 0:3e074f9f4063 147 myled1 = 1;
fyousuf3 0:3e074f9f4063 148 myled2 = 1;
fyousuf3 0:3e074f9f4063 149 myled3 = 1;
fyousuf3 0:3e074f9f4063 150 myled4 = 1;
fyousuf3 0:3e074f9f4063 151 }
fyousuf3 0:3e074f9f4063 152 else { // Otherwise just stop
fyousuf3 0:3e074f9f4063 153 xbee1.putc ('I');
fyousuf3 0:3e074f9f4063 154
fyousuf3 0:3e074f9f4063 155 myled1 = 0;
fyousuf3 0:3e074f9f4063 156 myled2 = 0;
fyousuf3 0:3e074f9f4063 157 myled3 = 0;
fyousuf3 0:3e074f9f4063 158 myled4 = 0;
fyousuf3 0:3e074f9f4063 159 }
fyousuf3 0:3e074f9f4063 160
fyousuf3 0:3e074f9f4063 161 }
fyousuf3 0:3e074f9f4063 162 }
fyousuf3 0:3e074f9f4063 163
fyousuf3 0:3e074f9f4063 164
fyousuf3 0:3e074f9f4063 165 float myConstrain(float val, float min, float max) {
fyousuf3 0:3e074f9f4063 166 float output = 0;
fyousuf3 0:3e074f9f4063 167
fyousuf3 0:3e074f9f4063 168 if (val < min) {
fyousuf3 0:3e074f9f4063 169 output = min;
fyousuf3 0:3e074f9f4063 170 } else if (val > max) {
fyousuf3 0:3e074f9f4063 171 output = max;
fyousuf3 0:3e074f9f4063 172 } else {
fyousuf3 0:3e074f9f4063 173 output = val;
fyousuf3 0:3e074f9f4063 174 }
fyousuf3 0:3e074f9f4063 175 return output;
fyousuf3 0:3e074f9f4063 176 }
fyousuf3 0:3e074f9f4063 177
fyousuf3 0:3e074f9f4063 178 float myScale(float val, float min, float max) { //not used
fyousuf3 0:3e074f9f4063 179 float output = (val - min) / (max - min);
fyousuf3 0:3e074f9f4063 180 return output;
fyousuf3 0:3e074f9f4063 181 }
fyousuf3 0:3e074f9f4063 182
fyousuf3 0:3e074f9f4063 183 float myConstrainScale(float val, float min, float max) {
fyousuf3 0:3e074f9f4063 184 float output = 0;
fyousuf3 0:3e074f9f4063 185
fyousuf3 0:3e074f9f4063 186 val = myConstrain(val, min, max);
fyousuf3 0:3e074f9f4063 187 output = (val - min) / (max - min);
fyousuf3 0:3e074f9f4063 188
fyousuf3 0:3e074f9f4063 189 return output;
fyousuf3 0:3e074f9f4063 190 }