main.cpp

00001  #include "mbed.h"
00002  #include "Servo.h"
00003  #include "MMA8452.h"
00004  #include "PinDetect.h"
00005 
00006 Serial pc(USBTX,USBRX);
00007 DigitalOut myLed(LED1);
00008 PinDetect pb1(p26);
00009 
00010 
00011 
00012 DigitalOut Red4(p5);
00013 DigitalOut Red3(p6);
00014 DigitalOut Yel2(p7);
00015 DigitalOut Grn1(p8);
00016 
00017 
00018 Servo _base(p21);
00019 Servo _shoulder(p22);
00020 Servo _elbow(p23);
00021 Servo _sword(p24);
00022 
00023 
00024 void countdown(){
00025     Red4 = 1;
00026     wait(.75);
00027     Red4 = 0;
00028     Red3 = 1;
00029     wait(.75);
00030     Red3 = 0;
00031     Yel2 = 1;
00032     wait(.75);
00033     Red4 = 1;
00034     Red3 = 1;
00035     Grn1 = 1;
00036     wait(.75);
00037     Red4 = 0;
00038     Red3 = 0;
00039     Yel2 = 0;
00040     Grn1 = 0;
00041   }
00042 
00043 
00044 void swing(){
00045     _sword.write(1.0);
00046     wait(.1);
00047     _sword.write(0.0);
00048   }
00049 
00050 int main() {
00051 
00052    pb1.mode(PullUp);
00053    int timer = 0;
00054    //float _baseRead = 0, _shoulderRead = 0, _elbowRead = 0;
00055    double x = 0, y = 0, z = 0;
00056    MMA8452 acc(p28, p27, 40000);  //instantiate an acc object 
00057    
00058    //set parameters -- use these and don't change
00059    acc.setBitDepth(MMA8452::BIT_DEPTH_12);
00060    acc.setDynamicRange(MMA8452::DYNAMIC_RANGE_4G);
00061    acc.setDataRate(MMA8452::RATE_100);
00062    countdown();
00063    
00064    while(1) {
00065     
00066 
00067       acc.readXYZGravity(&x,&y,&z); //notice this is passed by reference use pointers
00068 
00069       _base.write((-y+1)/2);
00070       _shoulder.write((-x+1)/2);
00071       //_elbow.write((x+1)/2);
00072 
00073       if(!pb1){
00074         myLed=1;
00075         swing();
00076         myLed=0;
00077     }
00078                           
00079       // You can uncomment this line to see the values coming off the MMA8452           
00080       //printf("(%.2f,%.2f,%.2f)", x,y,z);
00081       //printf("(%.5f,%.5f,%.5f)", _baseRead, _shoulderRead, _elbowRead);
00082       //printf("\n");
00083       
00084       timer++;      
00085       } //infinite while loop 
00086    } //end main