Advertisement
Martin_Toseski

Arduino Code

Mar 18th, 2024
1,051
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 3.32 KB | None | 0 0
  1. #include <Adafruit_LSM303_Accel.h>
  2. #include <Adafruit_Sensor.h>
  3. #include <Wire.h>
  4.  
  5. const int motor1pin1 = 4;             // motor 1 forwards
  6. const int motor1pin2 = 5;             // motor 1 backwards
  7.  
  8. const int motor2pin1 = 6;             // motor 2 forwards
  9. const int motor2pin2 = 7;             // motor 2 backwards
  10.  
  11. /* Assign a unique ID to this sensor at the same time */
  12. Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(54321);
  13.  
  14. void setup() {
  15. #ifndef ESP8266
  16.   while (!Serial)
  17.     ; // will pause Zero, Leonardo, etc until serial console opens
  18. #endif
  19.   Serial.begin(9600);
  20.   Serial.println("Accelerometer Test");
  21.   Serial.println("");
  22.  
  23.   /* Initialise the sensor */
  24.   if (!accel.begin()) {
  25.     /* There was a problem detecting the ADXL345 ... check your connections */
  26.     Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
  27.     while (1)
  28.       ;
  29.   }
  30.  
  31.   accel.setRange(LSM303_RANGE_4G);
  32.   Serial.print("Range set to: ");
  33.   lsm303_accel_range_t new_range = accel.getRange();
  34.   switch (new_range) {
  35.   case LSM303_RANGE_2G:
  36.     Serial.println("+- 2G");
  37.     break;
  38.   case LSM303_RANGE_4G:
  39.     Serial.println("+- 4G");
  40.     break;
  41.   case LSM303_RANGE_8G:
  42.     Serial.println("+- 8G");
  43.     break;
  44.   case LSM303_RANGE_16G:
  45.     Serial.println("+- 16G");
  46.     break;
  47.   }
  48.  
  49.   accel.setMode(LSM303_MODE_NORMAL);
  50.   Serial.print("Mode set to: ");
  51.   lsm303_accel_mode_t new_mode = accel.getMode();
  52.   switch (new_mode) {
  53.   case LSM303_MODE_NORMAL:
  54.     Serial.println("Normal");
  55.     break;
  56.   case LSM303_MODE_LOW_POWER:
  57.     Serial.println("Low Power");
  58.     break;
  59.   case LSM303_MODE_HIGH_RESOLUTION:
  60.     Serial.println("High Resolution");
  61.     break;
  62.   }
  63.  
  64.   pinMode(motor1pin1, OUTPUT);
  65.   pinMode(motor1pin2, OUTPUT);
  66.   pinMode(motor2pin1, OUTPUT);
  67.   pinMode(motor2pin2, OUTPUT);
  68. }
  69.  
  70. void loop() {
  71.   /* Get a new sensor event */
  72.   sensors_event_t event;
  73.   accel.getEvent(&event);
  74.  
  75.   /* Display the results (acceleration is measured in m/s^2) */
  76.   double currX = event.acceleration.x, currY = event.acceleration.y;
  77.   int angleX = (map (event.acceleration.x, -9.8, 9.8, -70, 70)), angleY = (map (event.acceleration.y, -9.8, 9.8, -70, 70));
  78.   Serial.print("X:");
  79.   Serial.print(angleX);
  80.   Serial.print(" ");
  81.   Serial.print("Y:");
  82.   Serial.print(angleY);
  83.   Serial.print(" ");
  84.   Serial.println("degrees");
  85.  
  86.   int speed1 = map(abs(angleX), 0, 70, 120, 255), speed2 = map(abs(angleY), 0, 70, 120, 255);
  87.  
  88.   if (angleX > 0) {
  89.     digitalWrite(motor2pin1, HIGH);
  90.     digitalWrite(motor1pin1, HIGH);
  91.     digitalWrite(motor1pin2, 0);
  92.     digitalWrite(motor2pin2, 0);
  93.   } else if (angleX == 0) {
  94.     digitalWrite(motor2pin1, 0);
  95.     digitalWrite(motor1pin1, 0);
  96.   }  else {
  97.     digitalWrite(motor2pin2, HIGH);
  98.     digitalWrite(motor1pin2, HIGH);
  99.     digitalWrite(motor1pin1, 0);
  100.     digitalWrite(motor2pin1, 0);
  101.   }
  102.  
  103.   if (angleY > 0) {
  104.     digitalWrite(motor1pin1, HIGH);
  105.     digitalWrite(motor2pin2, HIGH);
  106.     digitalWrite(motor2pin1, 0);
  107.     digitalWrite(motor1pin2, 0);
  108.   } else if (angleY == 0) {
  109.     digitalWrite(motor1pin1, 0);
  110.     digitalWrite(motor2pin2, 0);
  111.   }  else {
  112.     digitalWrite(motor1pin2, HIGH);
  113.     digitalWrite(motor2pin1, HIGH);
  114.     digitalWrite(motor2pin2, 0);
  115.     digitalWrite(motor1pin1, 0);
  116.   }
  117.  
  118.   /* Delay before the next sample */
  119.   delay(500);
  120. }
  121.  
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement