That is the code i use in my project, consist in hand what is controlled by a glove with sensors flex. I will explain all the factors and the tips i use in my code.
Dependencies: flex_sensor mbed Servo
Here we have the code of the both sides of the project, the part will control the arm and the part will move our arm.
gloveSensorSend/gloveSensorSend.cpp@2:0f0535b697ce, 2015-07-01 (annotated)
- Committer:
- Ronjea
- Date:
- Wed Jul 01 08:54:01 2015 +0000
- Revision:
- 2:0f0535b697ce
change;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Ronjea | 2:0f0535b697ce | 1 | #include "mbed.h" |
Ronjea | 2:0f0535b697ce | 2 | #include "flex_sensor.h" |
Ronjea | 2:0f0535b697ce | 3 | |
Ronjea | 2:0f0535b697ce | 4 | Serial xbee1(PA_11, PA_12); //Initalise xbee_lib |
Ronjea | 2:0f0535b697ce | 5 | Serial pc(USBTX, USBRX); //Initalise PC serial comms |
Ronjea | 2:0f0535b697ce | 6 | |
Ronjea | 2:0f0535b697ce | 7 | // Declaramos los pines analogicos de los cuales vamos a leer los valores que necesitamos |
Ronjea | 2:0f0535b697ce | 8 | flex_sensor chico_flex(A0); |
Ronjea | 2:0f0535b697ce | 9 | flex_sensor gordo_flex(A1); |
Ronjea | 2:0f0535b697ce | 10 | flex_sensor indice_flex(A2); |
Ronjea | 2:0f0535b697ce | 11 | flex_sensor corte_flex(A3); |
Ronjea | 2:0f0535b697ce | 12 | flex_sensor anillo_flex(A4); |
Ronjea | 2:0f0535b697ce | 13 | |
Ronjea | 2:0f0535b697ce | 14 | //Variable donde guardaremos de la posicion de nuestros dedos y con las que vamos a operar |
Ronjea | 2:0f0535b697ce | 15 | int chicoPosicion, gordoPosicion, anilloPosicion, indicePosicion, cortePosicion, |
Ronjea | 2:0f0535b697ce | 16 | chicoCerrado, gordoCerrado, anilloCerrado, indiceCerrado, corteCerrado, |
Ronjea | 2:0f0535b697ce | 17 | chicoAbierto, gordoAbierto, anilloAbierto, indiceAbierto, corteAbierto; |
Ronjea | 2:0f0535b697ce | 18 | /* Estas variables son las que vamos a mandar a traves del XBee, como solo se pueden mandar bytes |
Ronjea | 2:0f0535b697ce | 19 | y nuestro numero a enviar es de menos de 1 byte siempre le hacemos la conversion al final*/ |
Ronjea | 2:0f0535b697ce | 20 | int8_t chicoPosicionMandar, gordoPosicionMandar, anilloPosicionMandar, |
Ronjea | 2:0f0535b697ce | 21 | indicePosicionMandar, cortePosicionMandar; |
Ronjea | 2:0f0535b697ce | 22 | unsigned int hextoint(const char *hex); |
Ronjea | 2:0f0535b697ce | 23 | uint8_t reescala(int value, int inMin, int inMax, int outMin, int outMax) { |
Ronjea | 2:0f0535b697ce | 24 | return (value - inMin) * (outMax - outMin) / (inMax - inMin) + outMin; |
Ronjea | 2:0f0535b697ce | 25 | } |
Ronjea | 2:0f0535b697ce | 26 | |
Ronjea | 2:0f0535b697ce | 27 | int main() { |
Ronjea | 2:0f0535b697ce | 28 | xbee1.baud(38400); |
Ronjea | 2:0f0535b697ce | 29 | chicoAbierto = 3600; |
Ronjea | 2:0f0535b697ce | 30 | gordoAbierto = 3200; |
Ronjea | 2:0f0535b697ce | 31 | anilloAbierto = 3220; |
Ronjea | 2:0f0535b697ce | 32 | indiceAbierto = 3400; |
Ronjea | 2:0f0535b697ce | 33 | corteAbierto = 3400; |
Ronjea | 2:0f0535b697ce | 34 | |
Ronjea | 2:0f0535b697ce | 35 | chicoCerrado = chico_flex.get_raw_value(); |
Ronjea | 2:0f0535b697ce | 36 | gordoCerrado = gordo_flex.get_raw_value(); |
Ronjea | 2:0f0535b697ce | 37 | anilloCerrado = anillo_flex.get_raw_value(); |
Ronjea | 2:0f0535b697ce | 38 | indiceCerrado = indice_flex.get_raw_value(); |
Ronjea | 2:0f0535b697ce | 39 | corteCerrado = corte_flex.get_raw_value(); |
Ronjea | 2:0f0535b697ce | 40 | |
Ronjea | 2:0f0535b697ce | 41 | while (1) { |
Ronjea | 2:0f0535b697ce | 42 | |
Ronjea | 2:0f0535b697ce | 43 | //Traducimos el valor analogico de entrada del sensor a grados |
Ronjea | 2:0f0535b697ce | 44 | //con el reescalado y asi obtener la posicion |
Ronjea | 2:0f0535b697ce | 45 | //Estos son los valores que van a ser mandados |
Ronjea | 2:0f0535b697ce | 46 | chicoPosicion = chico_flex.get_raw_value(); |
Ronjea | 2:0f0535b697ce | 47 | gordoPosicion = gordo_flex.get_raw_value(); |
Ronjea | 2:0f0535b697ce | 48 | anilloPosicion = anillo_flex.get_raw_value(); |
Ronjea | 2:0f0535b697ce | 49 | indicePosicion = indice_flex.get_raw_value(); |
Ronjea | 2:0f0535b697ce | 50 | cortePosicion = corte_flex.get_raw_value(); |
Ronjea | 2:0f0535b697ce | 51 | pc.printf("Valor minimo o maximo indice %d\r\n", indicePosicion); |
Ronjea | 2:0f0535b697ce | 52 | pc.printf("Valor minimo o maximo gordo %d\r\n", gordoPosicion); |
Ronjea | 2:0f0535b697ce | 53 | pc.printf("Valor minimo o maximo anillo %d\r\n", anilloPosicion); |
Ronjea | 2:0f0535b697ce | 54 | pc.printf("Valor minimo o maximo chico %d\r\n", chicoPosicion); |
Ronjea | 2:0f0535b697ce | 55 | |
Ronjea | 2:0f0535b697ce | 56 | //Abierto |
Ronjea | 2:0f0535b697ce | 57 | if (gordoPosicion > 3200) { |
Ronjea | 2:0f0535b697ce | 58 | gordoAbierto = 3200; |
Ronjea | 2:0f0535b697ce | 59 | gordoPosicion = gordoAbierto; |
Ronjea | 2:0f0535b697ce | 60 | } else if (gordoPosicion < gordoAbierto) { |
Ronjea | 2:0f0535b697ce | 61 | gordoAbierto = gordoPosicion; |
Ronjea | 2:0f0535b697ce | 62 | } |
Ronjea | 2:0f0535b697ce | 63 | if (indicePosicion > 3300) { |
Ronjea | 2:0f0535b697ce | 64 | indiceAbierto = 3300; |
Ronjea | 2:0f0535b697ce | 65 | indicePosicion = indiceAbierto; |
Ronjea | 2:0f0535b697ce | 66 | } else if (indicePosicion > indiceAbierto) { |
Ronjea | 2:0f0535b697ce | 67 | indiceAbierto = indicePosicion; |
Ronjea | 2:0f0535b697ce | 68 | } |
Ronjea | 2:0f0535b697ce | 69 | if (cortePosicion > 3400) { |
Ronjea | 2:0f0535b697ce | 70 | |
Ronjea | 2:0f0535b697ce | 71 | corteAbierto = 3400; |
Ronjea | 2:0f0535b697ce | 72 | cortePosicion = corteAbierto; |
Ronjea | 2:0f0535b697ce | 73 | } else if (cortePosicion > corteAbierto) { |
Ronjea | 2:0f0535b697ce | 74 | corteAbierto = cortePosicion; |
Ronjea | 2:0f0535b697ce | 75 | } |
Ronjea | 2:0f0535b697ce | 76 | if (anilloPosicion > 3220) { |
Ronjea | 2:0f0535b697ce | 77 | anilloAbierto = 3220; |
Ronjea | 2:0f0535b697ce | 78 | anilloPosicion = anilloAbierto; |
Ronjea | 2:0f0535b697ce | 79 | } else if (anilloPosicion > anilloAbierto) { |
Ronjea | 2:0f0535b697ce | 80 | anilloAbierto = anilloPosicion; |
Ronjea | 2:0f0535b697ce | 81 | } |
Ronjea | 2:0f0535b697ce | 82 | if (chicoPosicion > 3600) { |
Ronjea | 2:0f0535b697ce | 83 | chicoAbierto = 3600; |
Ronjea | 2:0f0535b697ce | 84 | chicoPosicion = chicoAbierto; |
Ronjea | 2:0f0535b697ce | 85 | } else if (chicoPosicion > chicoAbierto) { |
Ronjea | 2:0f0535b697ce | 86 | chicoAbierto = chicoPosicion; |
Ronjea | 2:0f0535b697ce | 87 | } |
Ronjea | 2:0f0535b697ce | 88 | |
Ronjea | 2:0f0535b697ce | 89 | //Cerrado |
Ronjea | 2:0f0535b697ce | 90 | if (gordoPosicion < 2450) { |
Ronjea | 2:0f0535b697ce | 91 | gordoCerrado = 2450; |
Ronjea | 2:0f0535b697ce | 92 | gordoPosicion = gordoCerrado; |
Ronjea | 2:0f0535b697ce | 93 | } else if (gordoPosicion < gordoCerrado) { |
Ronjea | 2:0f0535b697ce | 94 | gordoCerrado = gordoPosicion; |
Ronjea | 2:0f0535b697ce | 95 | } |
Ronjea | 2:0f0535b697ce | 96 | if (indicePosicion < 2500) { |
Ronjea | 2:0f0535b697ce | 97 | indiceCerrado = 2500; |
Ronjea | 2:0f0535b697ce | 98 | indicePosicion = indiceCerrado; |
Ronjea | 2:0f0535b697ce | 99 | } else if (indicePosicion < indiceCerrado) { |
Ronjea | 2:0f0535b697ce | 100 | indiceCerrado = indicePosicion; |
Ronjea | 2:0f0535b697ce | 101 | |
Ronjea | 2:0f0535b697ce | 102 | } |
Ronjea | 2:0f0535b697ce | 103 | if (cortePosicion < 2500) { |
Ronjea | 2:0f0535b697ce | 104 | corteCerrado = 2500; |
Ronjea | 2:0f0535b697ce | 105 | cortePosicion = corteCerrado; |
Ronjea | 2:0f0535b697ce | 106 | } else if (cortePosicion < corteCerrado) { |
Ronjea | 2:0f0535b697ce | 107 | corteCerrado = cortePosicion; |
Ronjea | 2:0f0535b697ce | 108 | } |
Ronjea | 2:0f0535b697ce | 109 | if (anilloPosicion < 2360) { |
Ronjea | 2:0f0535b697ce | 110 | anilloCerrado = 2360; |
Ronjea | 2:0f0535b697ce | 111 | anilloPosicion = anilloCerrado; |
Ronjea | 2:0f0535b697ce | 112 | } else if (anilloPosicion < anilloCerrado) { |
Ronjea | 2:0f0535b697ce | 113 | anilloCerrado = anilloPosicion; |
Ronjea | 2:0f0535b697ce | 114 | } |
Ronjea | 2:0f0535b697ce | 115 | if (chicoPosicion < 2450) { |
Ronjea | 2:0f0535b697ce | 116 | chicoCerrado = 2450; |
Ronjea | 2:0f0535b697ce | 117 | chicoPosicion = chicoCerrado; |
Ronjea | 2:0f0535b697ce | 118 | } else if (chicoPosicion < chicoCerrado) { |
Ronjea | 2:0f0535b697ce | 119 | chicoCerrado = chicoPosicion; |
Ronjea | 2:0f0535b697ce | 120 | } |
Ronjea | 2:0f0535b697ce | 121 | //Cerrado significa que esta sin fuerza y abierto forzado |
Ronjea | 2:0f0535b697ce | 122 | chicoPosicionMandar = reescala(chicoPosicion, chicoCerrado, |
Ronjea | 2:0f0535b697ce | 123 | chicoAbierto, -10, 90); |
Ronjea | 2:0f0535b697ce | 124 | gordoPosicionMandar = reescala(gordoPosicion, gordoCerrado, |
Ronjea | 2:0f0535b697ce | 125 | gordoAbierto, -10, 90); |
Ronjea | 2:0f0535b697ce | 126 | anilloPosicionMandar = reescala(anilloPosicion, anilloCerrado, |
Ronjea | 2:0f0535b697ce | 127 | anilloAbierto, -25, 90); |
Ronjea | 2:0f0535b697ce | 128 | indicePosicionMandar = reescala(indicePosicion, indiceCerrado, |
Ronjea | 2:0f0535b697ce | 129 | indiceAbierto, -35, 90); |
Ronjea | 2:0f0535b697ce | 130 | cortePosicionMandar = reescala(cortePosicion, corteCerrado, |
Ronjea | 2:0f0535b697ce | 131 | corteAbierto, -20, 90); |
Ronjea | 2:0f0535b697ce | 132 | pc.printf("%d", cortePosicionMandar); |
Ronjea | 2:0f0535b697ce | 133 | |
Ronjea | 2:0f0535b697ce | 134 | xbee1.printf("%c%c%c%c%c%c%c", 0x7E, chicoPosicionMandar, |
Ronjea | 2:0f0535b697ce | 135 | gordoPosicionMandar, anilloPosicionMandar, indicePosicionMandar, |
Ronjea | 2:0f0535b697ce | 136 | cortePosicionMandar, 0x0D); |
Ronjea | 2:0f0535b697ce | 137 | pc.printf("%c,%d,%c", 0x7E, chicoPosicionMandar, 0x0D); |
Ronjea | 2:0f0535b697ce | 138 | |
Ronjea | 2:0f0535b697ce | 139 | wait_ms(30); |
Ronjea | 2:0f0535b697ce | 140 | |
Ronjea | 2:0f0535b697ce | 141 | } |
Ronjea | 2:0f0535b697ce | 142 | } |