main.cpp

00001 #include "meArm.h"
00002 #include "Servo.h"
00003 #include "MMA8452.h"
00004 #include "mbed.h"
00005 #include "PinDetect.h"
00006 
00007 meArm arm;
00008 
00009 Serial pc(USBTX,USBRX);
00010 
00011 DigitalOut myLed(LED1);
00012 PinDetect pb1(p20);
00013 
00014 MMA8452 mma(p28, p27, 100000);
00015 int x, y, z;
00016  
00017 //Change these values if accelerometer reading are different:
00018 //How far the accerometer is tilted before starting to move the arm:
00019 const int MovementThreshold = 18;
00020  
00021 //The average zero acceleration values read from the accelerometer for each axis:
00022 const int ZeroXValue = 0;
00023 const int ZeroYValue = 0;
00024 const int ZeroZValue = 0;
00025  
00026 //The maximum (positive) acceleration values read from the accelerometer for each axis:
00027 const int MaxXValue = 4096;
00028 const int MaxYValue = 4096;
00029 const int MaxZValue = 4096;
00030  
00031 //The minimum (negative) acceleration values read from the accelerometer for each axis:
00032 const int MinXValue = -4096;
00033 const int MinYValue = -4096;
00034 const int MinZValue = -4096;
00035  
00036 //The sign of the arm movement relative to the acceleration.
00037 //If arm is going in the opposite direction you think it should go, change the sign for the appropriate axis.
00038 const int XSign = 1;
00039 const int YSign = 1;
00040 const int ZSign = 1;
00041  
00042 //The maximum speed in each axis (x and y)
00043 //that the arm should move. Set this to a higher or lower number if the arm does not move fast enough or is too fast.
00044 const int MaxArmMovement = 50;  
00045  
00046 //This reduces the 'twitchiness' of the cursor by calling a delay function at the end of the main loop.
00047 //There are better way to do this without delaying the whole microcontroller, but that is left for another tutorial or project.
00048 const int ArmDelay = .001;
00049 
00050 //Function to process the acclerometer data
00051 //and send mouse movement information via USB
00052 void processAccelerometer(int16_t XReading, int16_t YReading, int16_t ZReading)
00053 {
00054   //Initialize values for the mouse cursor movement.
00055   int16_t ArmXMovement = 0;
00056   int16_t ArmYMovement = 0;
00057   int16_t ArmZMovement = 0;
00058   
00059   //Calculate mouse movement
00060   //If the analog X reading is ouside of the zero threshold...
00061   if( MovementThreshold < abs( XReading - ZeroXValue ) ){
00062     //...calculate X mouse movement based on how far the X acceleration is from its zero value.
00063     ArmXMovement = XSign * ( ( ( (float)( 2 * MaxArmMovement ) / ( MaxXValue - MinXValue ) ) * ( XReading - MinXValue ) ) - MaxArmMovement );
00064     //it could use some improvement, like making it trigonometric.
00065   }  else  {
00066     //Within the zero threshold, the cursor does not move in the X.
00067     ArmXMovement = 0;
00068   }
00069  
00070   //If the analog Y reading is ouside of the zero threshold... 
00071   if( MovementThreshold < abs( YReading - ZeroYValue ) ){
00072     //...calculate Y mouse movement based on how far the Y acceleration is from its zero value.
00073     ArmYMovement = YSign * ( ( ( (float)( 2 * MaxArmMovement ) / ( MaxYValue - MinYValue ) ) * ( YReading - MinYValue ) ) - MaxArmMovement );
00074     //it could use some improvement, like making it trigonometric.
00075   }  else  {
00076     //Within the zero threshold, the cursor does not move in the Y.
00077     ArmYMovement = 0;
00078   }
00079 
00080     //Calculate mouse movement
00081   //If the analog Z reading is ouside of the zero threshold...
00082   if( MovementThreshold < abs( ZReading - ZeroZValue ) )
00083   {
00084     //...calculate Z mouse movement based on how far the Z acceleration is from its zero value.
00085     ArmZMovement = ZSign * ( ( ( (float)( 2 * MaxArmMovement ) / ( MaxZValue - MinZValue ) ) * ( ZReading - MinZValue ) ) - MaxArmMovement );
00086     //it could use some improvement, like making it trigonometric.
00087   }
00088   else
00089   {
00090     //Within the zero threshold, the cursor does not move in the X.
00091     ArmZMovement = 0;
00092   }
00093   arm.gotoPoint(ArmXMovement, ArmYMovement, ArmZMovement);  // otherwise just move mouse
00094 }
00095 
00096 void swing(){
00097     arm.openGripper();
00098     arm.closeGripper();
00099   }
00100   
00101 int main() { 
00102   pb1.mode(PullUp);
00103   mma.readXYZCounts(&x, &y, &z);    // get an initial read   
00104   arm.begin();               // Initialize arm
00105 
00106   while(1) {
00107     
00108     if(!pb1){
00109         myLed=1;
00110         swing();
00111         myLed=0;
00112     }
00113     
00114     mma.readXYZCounts(&x, &y, &z);  //   // Read the 'raw' data in 14-bit counts
00115     printf("\n(%.2d,%.2d,%.2d)", x,y,z);   
00116     processAccelerometer(x,y,z);  // Work with the read data    
00117     wait(ArmDelay);  // wait until next reading - was 500 in Adafruit example
00118   }
00119 }