D. Damato / Mbed 2 deprecated Exp5_ForceFeedback

Exp5_ForceFeedback

Dependencies:   TextLCD mbed Servo

main.cpp@1:3168e55bf2da, 2011-08-03 (annotated)

Committer:
ddamato31
Date:
Wed Aug 03 05:58:23 2011 +0000
Revision:
1:3168e55bf2da
Parent:
0:41736722ccb8 

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


ddamato31

0:41736722ccb8


     1

// THis program will grip the paintball in an cyclic fashion





ddamato31

0:41736722ccb8


     2

//





ddamato31

0:41736722ccb8


     3

// IMPORTANT: Update LINE 37 and LINE 47 with suitable values based on your sensor calibration





ddamato31

0:41736722ccb8


     4

//





ddamato31

0:41736722ccb8


     5

// Run and then modify (i.e. Line 25 and Line 56) of program to grip at a set level.





ddamato31

0:41736722ccb8


     6

//





ddamato31

0:41736722ccb8


     7

#include "mbed.h"





ddamato31

0:41736722ccb8


     8

#include "math.h"





ddamato31

0:41736722ccb8


     9

#include "TextLCD.h"





ddamato31

0:41736722ccb8


    10







ddamato31

0:41736722ccb8


    11

AnalogIn FSR(p17);





ddamato31

0:41736722ccb8


    12

TextLCD lcd(p24, p26, p27, p28, p29, p30);





ddamato31

0:41736722ccb8


    13

PwmOut Servo(p22);





ddamato31

0:41736722ccb8


    14

float CalEq;





ddamato31

0:41736722ccb8


    15







ddamato31

0:41736722ccb8


    16

int main() {





ddamato31

0:41736722ccb8


    17







ddamato31

0:41736722ccb8


    18

// Calibrate Servo





ddamato31

0:41736722ccb8


    19

int offset = 1;





ddamato31

0:41736722ccb8


    20

     Servo.period_us(20000);





ddamato31

0:41736722ccb8


    21

     Servo.pulsewidth_us(1500);





ddamato31

0:41736722ccb8


    22







ddamato31

0:41736722ccb8


    23

// Force average initialization and index





ddamato31

1:3168e55bf2da


    24

    double ForceV[10] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};





ddamato31

0:41736722ccb8


    25

    int i; i = 0;





ddamato31

0:41736722ccb8


    26

    int squeeze;





ddamato31

0:41736722ccb8


    27

    squeeze = 1250;





ddamato31

0:41736722ccb8


    28

    





ddamato31

0:41736722ccb8


    29

    while(1) {





ddamato31

0:41736722ccb8


    30

    





ddamato31

0:41736722ccb8


    31

    // Index and reset Motor Average Counter





ddamato31

0:41736722ccb8


    32

       if(i > 9)





ddamato31

0:41736722ccb8


    33

       i = 0;





ddamato31

0:41736722ccb8


    34

       





ddamato31

0:41736722ccb8


    35

       





ddamato31

1:3168e55bf2da


    36

       double FSRconst = FSR;





ddamato31

1:3168e55bf2da


    37

       ForceV[i] = 6.3713*pow(97.6207, FSRconst) ;          // Use AnalogIn (FSR) reading in an equation of Force versus FSR values





ddamato31

0:41736722ccb8


    38

       float ForceAvg = (ForceV[0] + ForceV[1] + ForceV[2] + ForceV[3] + ForceV[4] + ForceV[5] + ForceV[6] + ForceV[7] + ForceV[8] + ForceV[9]) / 10 ;





ddamato31

0:41736722ccb8


    39

       lcd.cls();





ddamato31

0:41736722ccb8


    40

       lcd.locate(0, 0);





ddamato31

0:41736722ccb8


    41

       lcd.printf("Calibrate FSR");





ddamato31

0:41736722ccb8


    42

       lcd.locate(0, 1);





ddamato31

0:41736722ccb8


    43

       lcd.printf("V=%f", ForceAvg);





ddamato31

0:41736722ccb8


    44

       ++i;





ddamato31

0:41736722ccb8


    45

      





ddamato31

0:41736722ccb8


    46

       float Fmax;





ddamato31

0:41736722ccb8


    47

       Fmax = 140.0;





ddamato31

0:41736722ccb8


    48

              





ddamato31

0:41736722ccb8


    49

       if(ForceAvg > Fmax && squeeze > 1250){





ddamato31

0:41736722ccb8


    50

       squeeze = squeeze - offset;





ddamato31

0:41736722ccb8


    51

       Servo.pulsewidth_us(squeeze);





ddamato31

0:41736722ccb8


    52

       }





ddamato31

0:41736722ccb8


    53

       else if(ForceAvg < Fmax && squeeze < 1750){





ddamato31

0:41736722ccb8


    54

       squeeze = squeeze + offset;





ddamato31

0:41736722ccb8


    55

       Servo.pulsewidth_us(squeeze);





ddamato31

0:41736722ccb8


    56

       }





ddamato31

0:41736722ccb8


    57

       else





ddamato31

0:41736722ccb8


    58

       squeeze = 1250;





ddamato31

0:41736722ccb8


    59

       





ddamato31

0:41736722ccb8


    60

       wait(0.01);    





ddamato31

0:41736722ccb8


    61

    }





ddamato31

0:41736722ccb8


    62

}





ddamato31

0:41736722ccb8


    63







ddamato31

0:41736722ccb8


    64







ddamato31

0:41736722ccb8


    65

 





ddamato31

0:41736722ccb8


    66