/*
Arduino code for physiotherapy device for the hand
Written by Tom Verstraten (VUB)
E-mail: toverstr@vub.ac.be
Last update: 25-5-2011

The circuit for the LCD:
 * LCD RS pin (pin 4) to digital pin 12
 * LCD Enable pin (pin 6) to digital pin 11
 * LCD D4 pin (pin 11) to digital pin 5
 * LCD D5 pin to digital pin 4
 * LCD D6 pin to digital pin 3
 * LCD D7 pin (pin 14)to digital pin 2
 * LCD R/W (pin 5) pin to ground
 * 10K resistor:
 *   ends to +5V and ground 
 *   wiper (pin 3) to LCD VO pin (pin 3)

 */

// Include the LCD and servo library code:
#include <LiquidCrystal.h>
#include "Servoplus.h" 

// Define pins
/* Output */
const int rsPin=12;      // RS on LCD
const int enablePin=11;  // enable on LCD
const int D4Pin=5;       // DB4 on LCD
const int D5Pin=4;       // DB5 on LCD
const int D6Pin=3;       // DB6 on LCD
const int D7Pin=2;       // DB7 on LCD
const int servoPin=9;    // servo PWM
const int greenLedPin=13; // Green LED
/* Input */
const int rotPotPin=A1;  // rotating potentiometer
const int knopPin=10;    // push button
const int linPotPin=A0;  // linear potentiometer

// Initialization variables
const int linPotTreshMax=700;  // when linear potentiometer passes this treshold, handle is squeezed sufficiently far
const int linPotTreshMin=300;  // when linear potentiometer passes this treshold, handle is released sufficiently far
int rotPotValue = 0;         // value read from the pot for settings
int servoValue = 0;          // value output to the PWM for the servo
int weerstLcdValue = 0;      // resistance value printed on LCD
int linPotValue=0;           // value read from linear potentiometer measuring handle displacement
int squeezeTresh=20;         // number of squeezes
int progress=0;

LiquidCrystal lcd(rsPin, enablePin, D4Pin, D5Pin, D6Pin, D7Pin);  // creates variable lcd , syntax LiquidCrystal(rs, enable, d4, d5, d6, d7) 
Servo myservo;              // create servo object to control a servo

void setup() {
  // set up the LCD's number of columns (16) and rows (2): 
  lcd.begin(16, 2);
  lcd.print("Device on");
  pinMode(knopPin, INPUT);
  pinMode(greenLedPin, OUTPUT);  
  myservo.attach(servoPin);
}

void loop() {
 boolean finished=false;
 int squeezes=0;      // number of squeezes
 int knopValue=0;     // Value read from push button


  delay(2000);
 digitalWrite(greenLedPin, LOW); // set the green LED off


  // Set resistance
    lcd.clear();
    lcd.print("Force level?");
    // set the cursor to column 0, line 1
    // (note: line 1 is the second row, since counting begins with 0):
    lcd.setCursor(0, 1);
    // Turn off the blinking cursor:
    lcd.noBlink();
 
    while(knopValue==0){// test potentiometer until value is confirmed by pressing knopPin
    // Read the analog in value:
      rotPotValue = analogRead(rotPotPin);
    // Set  the lcd resistance value    
      // Map it to the range of the LCD (range from 0 to 10):
      weerstLcdValue = map(rotPotValue, 0, 1023, 0, 10);
      // Print resistance on LCD
      lcd.setCursor(0, 1);
      lcd.print(weerstLcdValue);
      lcd.print("  ");
      // Read if push button is pushed
      knopValue=digitalRead(knopPin);
    }    
    
  // Set the servomotor
   // Map it to use it with the servo (value between 0 and 180,
   // with 180 being the maximum amount of degrees the shaft can rotate around) 
     servoValue = map(rotPotValue, 0, 1023, 180, 0); 
   // Send to servo
     myservo.write(servoValue);      

  delay(1000);
  knopValue=0;

  // Set number of squeezes
    lcd.clear();
    lcd.print("Nr. of squeezes?");
    // Turn on the blinking cursor:
    while(knopValue==0){ // test potentiometer until value is confirmed by pressing knopPin
      // Read the analog in value:
        rotPotValue = analogRead(rotPotPin);
      // Set  the lcd resistance value    
        // Map it to the range of the squeezes (range from 0 to 50):
        squeezeTresh = map(rotPotValue, 0, 1023, 1, 50);
        // Print number of squeezes on LCD
        lcd.setCursor(7, 1);
        lcd.print(squeezeTresh);
        lcd.print("  ");
        delay(50);
        // Read if push button is pushed
        knopValue = digitalRead(knopPin);
    }
    
    delay(1000);
    
    lcd.clear();
    lcd.print("Start squeezing!");
    lcd.setCursor(0, 1);
    delay(1000);
    lcd.clear();
    
  // Count number if squeezes until treshold is reached
    while(finished == false){ 
      // read handle displacement from linear potentiometer
      linPotValue = analogRead(linPotPin);
      //Plot progress bar
      Progress(linPotValue);
      
      if (linPotValue>=linPotTreshMax){
         // if squeezed, then update number of squeezes
          digitalWrite(greenLedPin, HIGH); // set the green LED on
          squeezes=squeezes+1;
          // Print number of squeezes on lcd
          lcd.setCursor(0, 1);
          lcd.print("Squeezes: ");
          lcd.print(squeezes);
          
          // Wait for handle to return
          while (linPotValue>=linPotTreshMin){ 
            // linPotTreshMin, because hysteresis needed
            // When linear potentiometer value is below linPotTreshMin, handle is released sufficiently
            linPotValue = analogRead(linPotPin);
            Progress(linPotValue);
          }
          digitalWrite(greenLedPin, LOW); // set the green LED off
          
          if (squeezes>=squeezeTresh){
            // Check if squeezes treshold is reached. If so, end while
            finished=true;
            lcd.clear();
            lcd.print("Well done!");
          }
      }
   }
}

// Function for plotting progress bar
 void Progress(int Value){
   // Map to number of bits on LCD
    progress=map(Value,138,836,0,17);
    // Print the progress bar on first row of lcd
    for(int i=0; i<16; i++){
      lcd.setCursor(i,0);
      if (i<progress){
        // Print black block (char 255)
        // See data sheet for list of characters:
        // http://docs-europe.electrocomponents.com/webdocs/0f25/0900766b80f25db8.pdf
        lcd.print((char)255);  
      }
      else{
        // erase subsequent bits
        lcd.print(" "); 
      }
    }
 }
