API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Feb 16th, 2013
26
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 6.77 KB | None | 0 0
raw download clone embed print report
  1. /* Monitor a 3 axis accelerometer and control servos to keep sensor level
  2.    If arm is upright then keep sensor level above arm, if inverted keep level below arm
  3.    Servos should not move out ouf range
  4.    All values and bargraphs from 0G to 1G displayed on 4 line by 20 char LCD
  5.    atmega328p is being used on nano arduino
  6.    Version 0.1 RifRaf 17-2-2013 - first version */
  7.  
  8. #include "hardware.h"
  9.  
  10. // Initialise the hardware
  11. void appInitHardware(void) {
  12.     initHardware();
  13. }
  14. // Initialise the software
  15. TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
  16.     rprintfInit(displayGetWriter(&display));
  17.     rprintf("ADXL345Accelerometer");  //Setup LCD for chars that will not change
  18.     displayGoto(&display,0,1);
  19.     rprintf("X      mG           ");
  20.     displayGoto(&display,0,2);
  21.     rprintf("Y      mG           ");
  22.     displayGoto(&display,0,3);
  23.     rprintf("Z      mG           ");
  24.     return 0;
  25. }
  26. // This is the main loop
  27. TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) {
  28.  
  29.     rprintfInit(displayGetWriter(&display));
  30.     //Check accelerometer and create value for X Y Z
  31.     accelerometerRead(acceleration);
  32.     int valX = acceleration.accelerometer.x_axis_mG;
  33.     int valY = acceleration.accelerometer.y_axis_mG;
  34.     int valZ = acceleration.accelerometer.z_axis_mG;
  35.  
  36.     if (valZ >=0) {  //Check to see that arm is upright
  37.         if (valX >=400) { //Long way off, move quick
  38.             DRIVE_SPEED speed=act_getSpeed(&servo2);
  39.             if (speed <= 122){ speed = speed + 5;}
  40.             act_setSpeed(&servo2,speed);
  41.         }
  42.         else if (valX >=200) {  //Getting closer, slow down
  43.             DRIVE_SPEED speed=act_getSpeed(&servo2);
  44.             if (speed <= 125){ speed = speed + 2;}
  45.             act_setSpeed(&servo2,speed);
  46.         }
  47.         else if (valX >=100) { //Close so go slower still
  48.             DRIVE_SPEED speed=act_getSpeed(&servo2);
  49.             if (speed <= 126){ speed ++;}
  50.             act_setSpeed(&servo2,speed);
  51.         }
  52.         else if (valX >=50) { //Close enough to just stop
  53.             DRIVE_SPEED speed=act_getSpeed(&servo2);
  54.             act_setSpeed(&servo2,speed);
  55.         }
  56.         if (valX <=-400) { //Next section is just the opposite of abuve
  57.             DRIVE_SPEED speed=act_getSpeed(&servo2);
  58.             if (speed >= -122){ speed = speed - 5;}
  59.             act_setSpeed(&servo2,speed);
  60.         }
  61.         else if (valX <=-200) {
  62.             DRIVE_SPEED speed=act_getSpeed(&servo2);
  63.             if (speed >= -125){ speed = speed - 2;}
  64.             act_setSpeed(&servo2,speed);
  65.         }
  66.         else if (valX <=-100)  {
  67.             DRIVE_SPEED speed=act_getSpeed(&servo2);
  68.             if (speed >= -126){ speed --;}
  69.             act_setSpeed(&servo2,speed);
  70.         }
  71.         else if (valX <=-50)  {
  72.             DRIVE_SPEED speed=act_getSpeed(&servo2);
  73.             act_setSpeed(&servo2,speed);
  74.         }
  75.     }
  76.    
  77.     if (valZ <=-1) {  // Check to see if upside down and do opposite of above for X axis
  78.         if (valX >=400) {
  79.             DRIVE_SPEED speed=act_getSpeed(&servo2);
  80.             if (speed <= 122){ speed = speed - 5;}
  81.             act_setSpeed(&servo2,speed);
  82.         }
  83.         else if (valX >=200) {
  84.             DRIVE_SPEED speed=act_getSpeed(&servo2);
  85.             if (speed <= 125){ speed = speed - 2;}
  86.             act_setSpeed(&servo2,speed);
  87.         }
  88.         else if (valX >=100) {
  89.             DRIVE_SPEED speed=act_getSpeed(&servo2);
  90.             if (speed <= 126){ speed --;}
  91.             act_setSpeed(&servo2,speed);
  92.         }
  93.         else if (valX >=50) {
  94.             DRIVE_SPEED speed=act_getSpeed(&servo2);
  95.             act_setSpeed(&servo2,speed);
  96.         }
  97.         if (valX <=-400) {
  98.             DRIVE_SPEED speed=act_getSpeed(&servo2);
  99.             if (speed >= -122){ speed = speed + 5;}
  100.             act_setSpeed(&servo2,speed);
  101.         }
  102.         else if (valX <=-200) {
  103.             DRIVE_SPEED speed=act_getSpeed(&servo2);
  104.             if (speed >= -125){ speed = speed + 2;}
  105.             act_setSpeed(&servo2,speed);
  106.         }
  107.         else if (valX <=-100)  {
  108.             DRIVE_SPEED speed=act_getSpeed(&servo2);
  109.             if (speed >= -126){ speed ++;}
  110.             act_setSpeed(&servo2,speed);
  111.         }
  112.         else if (valX <=-50)  {
  113.             DRIVE_SPEED speed=act_getSpeed(&servo2);
  114.             act_setSpeed(&servo2,speed);
  115.         }
  116.     }
  117.     displayGoto(&display,2,1);  // Print valX to LCD
  118.     rprintf("%d",valX);
  119.         if (valX < 0) {
  120.          valX = -valX;
  121.     }
  122.     displayHorizGraph(&display,10,1,valX,1000,10);
  123.  
  124.     if (valZ >=0) {  //See if right way up, control Y axis
  125.         if (valY >=400) {
  126.             DRIVE_SPEED speed=act_getSpeed(&servo3);
  127.             if (speed <= 122){ speed = speed + 5;}
  128.             act_setSpeed(&servo3,speed);
  129.         }
  130.         else if (valY >=200) {
  131.             DRIVE_SPEED speed=act_getSpeed(&servo3);
  132.             if (speed <= 125){ speed = speed + 2;}
  133.             act_setSpeed(&servo3,speed);
  134.         }
  135.         else if (valY >=100) {
  136.             DRIVE_SPEED speed=act_getSpeed(&servo3);
  137.             if (speed <= 126){ speed ++;}
  138.             act_setSpeed(&servo3,speed);
  139.         }
  140.         else if (valY >=50) {
  141.             DRIVE_SPEED speed=act_getSpeed(&servo3);
  142.             act_setSpeed(&servo3,speed);
  143.         }
  144.         if (valY <=-400) {
  145.             DRIVE_SPEED speed=act_getSpeed(&servo3);
  146.             if (speed >= -122){ speed = speed - 5;}
  147.             act_setSpeed(&servo3,speed);
  148.         }
  149.         else if (valY <=-200) {
  150.             DRIVE_SPEED speed=act_getSpeed(&servo3);
  151.             if (speed >= -125){ speed = speed - 2;}
  152.             act_setSpeed(&servo3,speed);
  153.         }
  154.         else if (valY <=-100)  {
  155.             DRIVE_SPEED speed=act_getSpeed(&servo3);
  156.             if (speed >= -126){ speed --;}
  157.             act_setSpeed(&servo3,speed);
  158.         }
  159.         else if (valY <=-50)  {
  160.             DRIVE_SPEED speed=act_getSpeed(&servo3);
  161.             act_setSpeed(&servo3,speed);
  162.         }
  163.     }  
  164.    
  165.     if (valZ <=-1) { //See if upside down, Control Y axis
  166.         if (valY >=400) {
  167.             DRIVE_SPEED speed=act_getSpeed(&servo3);
  168.             if (speed <= 122){ speed = speed - 5;}
  169.             act_setSpeed(&servo3,speed);
  170.         }
  171.         else if (valY >=200) {
  172.             DRIVE_SPEED speed=act_getSpeed(&servo3);
  173.             if (speed <= 125){ speed = speed - 2;}
  174.             act_setSpeed(&servo3,speed);
  175.         }
  176.         else if (valY >=100) {
  177.             DRIVE_SPEED speed=act_getSpeed(&servo3);
  178.             if (speed <= 126){ speed --;}
  179.             act_setSpeed(&servo3,speed);
  180.         }
  181.         else if (valY >=50) {
  182.             DRIVE_SPEED speed=act_getSpeed(&servo3);
  183.             act_setSpeed(&servo3,speed);
  184.         }
  185.         if (valY <=-400) {
  186.             DRIVE_SPEED speed=act_getSpeed(&servo3);
  187.             if (speed >= -122){ speed = speed + 5;}
  188.             act_setSpeed(&servo3,speed);
  189.         }
  190.         else if (valY <=-200) {
  191.             DRIVE_SPEED speed=act_getSpeed(&servo3);
  192.             if (speed >= -125){ speed = speed + 2;}
  193.             act_setSpeed(&servo3,speed);
  194.         }
  195.         else if (valY <=-100)  {
  196.             DRIVE_SPEED speed=act_getSpeed(&servo3);
  197.             if (speed >= -126){ speed ++;}
  198.             act_setSpeed(&servo3,speed);
  199.         }
  200.         else if (valY <=-50)  {
  201.             DRIVE_SPEED speed=act_getSpeed(&servo3);
  202.             act_setSpeed(&servo3,speed);
  203.         }
  204.     }  
  205.     displayGoto(&display,2,2);  //Display valY to LCD
  206.     rprintf("%d",valY);
  207.         if (valY < 0) {
  208.          valY = -valY;
  209.     }
  210.     displayHorizGraph(&display,10,2,valY,1000,10);
  211.  
  212.     displayGoto(&display,2,3);  //Display valZ to LCD
  213.     rprintf("%d",valZ);
  214.         if (valZ < 0) {
  215.          valZ = -valZ;
  216.     }
  217.     displayHorizGraph(&display,10,3,valZ,1000,10);
  218.  
  219.     delay_ms(20); // Pause so values remain on screen a bit then blank srceen values to get rid of them
  220.     displayGoto(&display,2,1);
  221.     rprintf("     ");
  222.     displayGoto(&display,2,2);
  223.     rprintf("     ");
  224.     displayGoto(&display,2,3);
  225.     rprintf("     ");
  226.  
  227.     return 0;
  228. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!