Diff: main.cpp

Revision:

0:41736722ccb8

Child:

1:3168e55bf2da

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Aug 02 22:48:23 2011 +0000
@@ -0,0 +1,66 @@
+// THis program will grip the paintball in an cyclic fashion
+//
+// IMPORTANT: Update LINE 37 and LINE 47 with suitable values based on your sensor calibration
+//
+// Run and then modify (i.e. Line 25 and Line 56) of program to grip at a set level.
+//
+#include "mbed.h"
+#include "math.h"
+#include "TextLCD.h"
+
+AnalogIn FSR(p17);
+TextLCD lcd(p24, p26, p27, p28, p29, p30);
+PwmOut Servo(p22);
+float CalEq;
+
+int main() {
+
+// Calibrate Servo
+int offset = 1;
+     Servo.period_us(20000);
+     Servo.pulsewidth_us(1500);
+
+// Force average initialization and index
+    float ForceV[10] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+    int i; i = 0;
+    int squeeze;
+    squeeze = 1250;
+    
+    while(1) {
+    
+    // Index and reset Motor Average Counter
+       if(i > 9)
+       i = 0;
+       
+       
+       //CalEq = 1.0;
+       ForceV[i] = 97.6207*exp(FSR) ;          // Use AnalogIn (FSR) reading in an equation of Force versus FSR values
+       float ForceAvg = (ForceV[0] + ForceV[1] + ForceV[2] + ForceV[3] + ForceV[4] + ForceV[5] + ForceV[6] + ForceV[7] + ForceV[8] + ForceV[9]) / 10 ;
+       lcd.cls();
+       lcd.locate(0, 0);
+       lcd.printf("Calibrate FSR");
+       lcd.locate(0, 1);
+       lcd.printf("V=%f", ForceAvg);
+       ++i;
+      
+       float Fmax;
+       Fmax = 140.0;
+              
+       if(ForceAvg > Fmax && squeeze > 1250){
+       squeeze = squeeze - offset;
+       Servo.pulsewidth_us(squeeze);
+       }
+       else if(ForceAvg < Fmax && squeeze < 1750){
+       squeeze = squeeze + offset;
+       Servo.pulsewidth_us(squeeze);
+       }
+       else
+       squeeze = 1250;
+       
+       wait(0.01);    
+    }
+}
+
+
+ 
+