ARC lab soft hand / Mbed 2 deprecated backup_demo

backup demo for touchtomorrow should the finger controller decide not to work.

Dependencies:   DebounceIn TextLCD mbed

main.cpp@0:5c5ab0d93961, 2014-06-14 (annotated)

Committer:
NotCras
Date:
Sat Jun 14 13:16:50 2014 +0000
Revision:
0:5c5ab0d93961
backup demo for touchtomorrow should the finger controller stop working. (it did)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NotCras 0:5c5ab0d93961 1 #include "mbed.h"
NotCras 0:5c5ab0d93961 2 #include "TextLCD.h"
NotCras 0:5c5ab0d93961 3 #include "DebounceIn.h"
NotCras 0:5c5ab0d93961 4
NotCras 0:5c5ab0d93961 5
NotCras 0:5c5ab0d93961 6 /********************MacValveControlwithLCD**************************
NotCras 0:5c5ab0d93961 7 What this code will do is implement the previous Mac Valve control code for automated testing with an LCD screen
NotCras 0:5c5ab0d93961 8 and the ability to determine the pressure control variable.
NotCras 0:5c5ab0d93961 9
NotCras 0:5c5ab0d93961 10 Automated test code will exist, however the next test phase will have to be initiated with a button press. Furthermore,
NotCras 0:5c5ab0d93961 11 I will still keep the pressure ++ after every test, because I still want the code to be as automated as possible.
NotCras 0:5c5ab0d93961 12
NotCras 0:5c5ab0d93961 13 I will also add in dual sensor capability, so I can get both flex sensor readings from one test. *Done
NotCras 0:5c5ab0d93961 14 */
NotCras 0:5c5ab0d93961 15
NotCras 0:5c5ab0d93961 16 #define VAL 0.1
NotCras 0:5c5ab0d93961 17
NotCras 0:5c5ab0d93961 18 LocalFileSystem local("local");
NotCras 0:5c5ab0d93961 19
NotCras 0:5c5ab0d93961 20 //analog out pin is 18 (the only analog out pin)
NotCras 0:5c5ab0d93961 21 AnalogOut pcontroller(p18);
NotCras 0:5c5ab0d93961 22
NotCras 0:5c5ab0d93961 23 //analog in pins are 19 and 20 (will read sensor values from here)
NotCras 0:5c5ab0d93961 24 AnalogIn voltIn(p19);
NotCras 0:5c5ab0d93961 25 AnalogIn voltIn2(p20);
NotCras 0:5c5ab0d93961 26
NotCras 0:5c5ab0d93961 27 //now to define the solenoids
NotCras 0:5c5ab0d93961 28 DigitalOut solenoid(p12);
NotCras 0:5c5ab0d93961 29
NotCras 0:5c5ab0d93961 30 //this is to define the replenish signal led
NotCras 0:5c5ab0d93961 31 DigitalOut led(LED1); // was formerly moveOn
NotCras 0:5c5ab0d93961 32 DigitalOut led2(LED2); // was formerly moveOn
NotCras 0:5c5ab0d93961 33
NotCras 0:5c5ab0d93961 34 //these are the buttons which will add or subtract values to the p value after each test
NotCras 0:5c5ab0d93961 35 DigitalIn pPlus(p13);
NotCras 0:5c5ab0d93961 36 DigitalIn pMinus(p14);
NotCras 0:5c5ab0d93961 37
NotCras 0:5c5ab0d93961 38 //this is to define the button to move to the next psi step
NotCras 0:5c5ab0d93961 39 InterruptIn incCurl(p21);
NotCras 0:5c5ab0d93961 40 InterruptIn decCurl(p22);
NotCras 0:5c5ab0d93961 41 InterruptIn eStop(p23);
NotCras 0:5c5ab0d93961 42
NotCras 0:5c5ab0d93961 43 //initialize serial for debugging
NotCras 0:5c5ab0d93961 44 Serial pc(USBTX, USBRX); // tx, rx
NotCras 0:5c5ab0d93961 45
NotCras 0:5c5ab0d93961 46 //initialize LCD for testing
NotCras 0:5c5ab0d93961 47 TextLCD lcd(p15, p16, p27, p28, p25, p24, TextLCD::LCD16x2); // rs, e, d4-d7
NotCras 0:5c5ab0d93961 48
NotCras 0:5c5ab0d93961 49 Timer t;
NotCras 0:5c5ab0d93961 50
NotCras 0:5c5ab0d93961 51 Ticker controller;
NotCras 0:5c5ab0d93961 52
NotCras 0:5c5ab0d93961 53 /**********************************************************GLOBALS****************************************************************************/
NotCras 0:5c5ab0d93961 54 float controlVal, p_term, i_term, d_term, correction, p;
NotCras 0:5c5ab0d93961 55 float kp, ki, kd, time_step, desired, actual;
NotCras 0:5c5ab0d93961 56 float E, last_E;
NotCras 0:5c5ab0d93961 57 int running = 1;
NotCras 0:5c5ab0d93961 58
NotCras 0:5c5ab0d93961 59
NotCras 0:5c5ab0d93961 60 /*****************************************************SUPPORT FUNCTIONS*********************************************************************/
NotCras 0:5c5ab0d93961 61
NotCras 0:5c5ab0d93961 62 //only works for pressure up to 33 psi (which is more than we'll need for one finger, so I will not address this problem)
NotCras 0:5c5ab0d93961 63 float pToVal(float pressure){
NotCras 0:5c5ab0d93961 64 float c,e;
NotCras 0:5c5ab0d93961 65 //convert pressure to analog control value
NotCras 0:5c5ab0d93961 66 e = pressure;
NotCras 0:5c5ab0d93961 67 c = e / 33; // (/10 to get voltage required, /3.3 to get control value)
NotCras 0:5c5ab0d93961 68 return c;
NotCras 0:5c5ab0d93961 69 }
NotCras 0:5c5ab0d93961 70
NotCras 0:5c5ab0d93961 71 void initController(void){
NotCras 0:5c5ab0d93961 72 kp = 200;
NotCras 0:5c5ab0d93961 73 ki = 20;
NotCras 0:5c5ab0d93961 74 kd = 1.25;
NotCras 0:5c5ab0d93961 75
NotCras 0:5c5ab0d93961 76 i_term = 0;
NotCras 0:5c5ab0d93961 77 time_step = 0.0001;
NotCras 0:5c5ab0d93961 78 desired = 1;
NotCras 0:5c5ab0d93961 79 E = 0;
NotCras 0:5c5ab0d93961 80 last_E = 0;
NotCras 0:5c5ab0d93961 81 }
NotCras 0:5c5ab0d93961 82
NotCras 0:5c5ab0d93961 83 /*****************************************************INTERRUPT FUNCTIONS*********************************************************************/
NotCras 0:5c5ab0d93961 84
NotCras 0:5c5ab0d93961 85 void flashLED(void){
NotCras 0:5c5ab0d93961 86 led = !led;
NotCras 0:5c5ab0d93961 87 }
NotCras 0:5c5ab0d93961 88
NotCras 0:5c5ab0d93961 89 void control(void){
NotCras 0:5c5ab0d93961 90 led = !led;
NotCras 0:5c5ab0d93961 91 //make sure desired value is within bounds
NotCras 0:5c5ab0d93961 92 if(desired <= 0.3){
NotCras 0:5c5ab0d93961 93 desired = 0.3;
NotCras 0:5c5ab0d93961 94 }
NotCras 0:5c5ab0d93961 95 if(desired >= 1){
NotCras 0:5c5ab0d93961 96 desired = 1;
NotCras 0:5c5ab0d93961 97 }
NotCras 0:5c5ab0d93961 98
NotCras 0:5c5ab0d93961 99 actual = voltIn.read();
NotCras 0:5c5ab0d93961 100 E = desired - actual;
NotCras 0:5c5ab0d93961 101
NotCras 0:5c5ab0d93961 102 p_term = kp * E;
NotCras 0:5c5ab0d93961 103 i_term += ki * E * time_step;
NotCras 0:5c5ab0d93961 104 d_term = kd * (E - last_E) / time_step;
NotCras 0:5c5ab0d93961 105
NotCras 0:5c5ab0d93961 106 correction = p_term + i_term + d_term;
NotCras 0:5c5ab0d93961 107 p = -1 * correction;
NotCras 0:5c5ab0d93961 108 last_E = E;
NotCras 0:5c5ab0d93961 109
NotCras 0:5c5ab0d93961 110 //bound output
NotCras 0:5c5ab0d93961 111 if(p > 25)
NotCras 0:5c5ab0d93961 112 p = 25;
NotCras 0:5c5ab0d93961 113 if(p < 0)
NotCras 0:5c5ab0d93961 114 p = 0;
NotCras 0:5c5ab0d93961 115
NotCras 0:5c5ab0d93961 116 //get the value needed to control the pressure from interrupt
NotCras 0:5c5ab0d93961 117 controlVal = pToVal(p);
NotCras 0:5c5ab0d93961 118 //use value found to control proportional controller
NotCras 0:5c5ab0d93961 119 pcontroller.write(controlVal);
NotCras 0:5c5ab0d93961 120 }
NotCras 0:5c5ab0d93961 121
NotCras 0:5c5ab0d93961 122 void increase(){
NotCras 0:5c5ab0d93961 123 p++;
NotCras 0:5c5ab0d93961 124
NotCras 0:5c5ab0d93961 125 if(p < 8){
NotCras 0:5c5ab0d93961 126 p = 8;
NotCras 0:5c5ab0d93961 127 }
NotCras 0:5c5ab0d93961 128
NotCras 0:5c5ab0d93961 129 if(p > 22){
NotCras 0:5c5ab0d93961 130 p = 22;
NotCras 0:5c5ab0d93961 131 }
NotCras 0:5c5ab0d93961 132
NotCras 0:5c5ab0d93961 133 wait(0.2);
NotCras 0:5c5ab0d93961 134 }
NotCras 0:5c5ab0d93961 135
NotCras 0:5c5ab0d93961 136 void decrease(){
NotCras 0:5c5ab0d93961 137 p--;
NotCras 0:5c5ab0d93961 138
NotCras 0:5c5ab0d93961 139 if(p < 7){
NotCras 0:5c5ab0d93961 140 p = 0;
NotCras 0:5c5ab0d93961 141 }
NotCras 0:5c5ab0d93961 142
NotCras 0:5c5ab0d93961 143 wait(0.2);
NotCras 0:5c5ab0d93961 144 }
NotCras 0:5c5ab0d93961 145
NotCras 0:5c5ab0d93961 146 void stop(void){
NotCras 0:5c5ab0d93961 147 pcontroller.write(0);
NotCras 0:5c5ab0d93961 148 running = 0;
NotCras 0:5c5ab0d93961 149 }
NotCras 0:5c5ab0d93961 150
NotCras 0:5c5ab0d93961 151 /************************************************************MAIN FUNCTION************************************************************************/
NotCras 0:5c5ab0d93961 152
NotCras 0:5c5ab0d93961 153 int main() {
NotCras 0:5c5ab0d93961 154
NotCras 0:5c5ab0d93961 155 //setup pullups
NotCras 0:5c5ab0d93961 156 incCurl.mode(PullUp);
NotCras 0:5c5ab0d93961 157 decCurl.mode(PullUp);
NotCras 0:5c5ab0d93961 158 eStop.mode(PullUp);
NotCras 0:5c5ab0d93961 159
NotCras 0:5c5ab0d93961 160 //attach interrupts
NotCras 0:5c5ab0d93961 161 incCurl.rise(&increase);
NotCras 0:5c5ab0d93961 162 decCurl.rise(&decrease);
NotCras 0:5c5ab0d93961 163 eStop.rise(&stop);
NotCras 0:5c5ab0d93961 164
NotCras 0:5c5ab0d93961 165 //fill the finger
NotCras 0:5c5ab0d93961 166 solenoid = 1;
NotCras 0:5c5ab0d93961 167
NotCras 0:5c5ab0d93961 168 float controlVal;
NotCras 0:5c5ab0d93961 169
NotCras 0:5c5ab0d93961 170 while(1){
NotCras 0:5c5ab0d93961 171 if(running){
NotCras 0:5c5ab0d93961 172 //get the value needed to control the pressure
NotCras 0:5c5ab0d93961 173 controlVal = pToVal(p);
NotCras 0:5c5ab0d93961 174
NotCras 0:5c5ab0d93961 175 //use value found to control proportional controller
NotCras 0:5c5ab0d93961 176 pcontroller.write(controlVal);
NotCras 0:5c5ab0d93961 177 }
NotCras 0:5c5ab0d93961 178 }
NotCras 0:5c5ab0d93961 179 }