What Do I Build Next? Part 3: 1Sheeld/Alamode RasPi Robot

/*Gamepad Shield Example
2This example shows an application on 1Sheeld's gamepad shield.
3
4******* This is modified to use the adafruit motorshield V2.3 *********
5
6OPTIONAL:
7To reduce the library compiled size and limit its memory usage, you
8can specify which shields you want to include in your sketch by
9defining CUSTOM_SETTINGS and the shields respective INCLUDE_ define.*/
10
11int ledPin = 13;
12#define CUSTOM_SETTINGS
13/* Use the game pad /
14/ Include 1Sheeld library. */
15#define INCLUDE_GAMEPAD_SHIELD
16#include <OneSheeld.h>
17//#include <AFMotor.h>
18#include <Adafruit_MotorShield.h> //for v2.3 motorshield
19
20Adafruit_MotorShield AFMS = Adafruit_MotorShield();
21
22// Motor objects, motor number on the motor shield board
23Adafruit_DCMotor *myMotor = AFMS.getMotor(1);
24Adafruit_DCMotor *myOtherMotor = AFMS.getMotor(2);
25
26void setup()
27{
28/* Start communication. */
29OneSheeld.begin();
30AFMS.begin();
31
32// The LED just works as an indicator that the 1Sheeld is working fine
33pinMode(ledPin, OUTPUT);
34myMotor->setSpeed(150);
35myOtherMotor->setSpeed(150);
36
37}
38void loop()
39{
40/* Always check the status of gamepad buttons. */
41if (GamePad.isDownPressed())
42{
43myMotor->run(BACKWARD);
44myOtherMotor->run(BACKWARD);
45
46digitalWrite(ledPin, HIGH);
47}
48else if (GamePad.isUpPressed())
49{
50myMotor->run(FORWARD);
51myOtherMotor->run(FORWARD);
52digitalWrite(ledPin, HIGH);
53}
54else if (GamePad.isLeftPressed())
55{
56myMotor->run(BACKWARD);
57myOtherMotor->run(FORWARD);
58digitalWrite(ledPin, HIGH);
59}
60else if (GamePad.isRightPressed())
61{
62myMotor->run(FORWARD);
63myOtherMotor->run(BACKWARD);
64digitalWrite(ledPin, HIGH);
65}
66else
67{
68myMotor->run(RELEASE);
69myOtherMotor->run(RELEASE);
70digitalWrite(ledPin, LOW);
71}
72}

/*Gamepad Shield Example
This example shows an application on 1Sheeld's gamepad shield.Listing from https://www.hackster.io/13766/super-easy-to-build-1sheeld-2-wheel-drive-robot-cb9b66

******* This is modified to use the adafruit motorshield V2.3 Created by Garry Grewer, *********
/*
        Pan-Tilt Controlled by Cell Phone
             by LAGSILVA - 14.Feb.2016
  (Application supported by 1SHEELD-Android for Arduino UNO)

  PS: X-axis ranges from 0 to 360 degrees, Y-axis ranges from -180 to 180 degrees
  and Z-axis ranges from -90 to 90 degrees.

*/

/*
        Pan-Tilt Controlled by Cell Phone
             by LAGSILVA - 14.Feb.2016
  (Application supported by 1SHEELD-Android for Arduino UNO)

  PS: X-axis ranges from 0 to 360 degrees, Y-axis ranges from -180 to 180 degrees
  and Z-axis ranges from -90 to 90 degrees.

*/
int ledPin = 13;
#define CUSTOM_SETTINGS
#define INCLUDE_ORIENTATION_SENSOR_SHIELD
#define INCLUDE_GAMEPAD_SHIELD

#include <OneSheeld.h>    // Include 1Sheeld Library
#include <Servo.h>        // Include Servo Library
#include <Adafruit_MotorShield.h> //for v2.3 motorshield

Adafruit_MotorShield AFMS = Adafruit_MotorShield();

// Motor objects, motor number on the motor shield board
Adafruit_DCMotor *myMotor = AFMS.getMotor(1);
Adafruit_DCMotor *myOtherMotor = AFMS.getMotor(2);
Adafruit_DCMotor *myMotor3 = AFMS.getMotor(3);
Adafruit_DCMotor *myOtherMotor4 = AFMS.getMotor(4);

Servo myservoX;           // Create servo object to control Orientation X
Servo myservoY;           // Create servo object to control Orientation Y

float angleX, angleXi, angleY;
float angleYi = -35;      // This value must be set with a trial to align the "zero" angle of your Tilt. In my case I used -35 for compensation.
boolean k = true;

void setup()
{

  OneSheeld.begin();
 AFMS.begin();

// The LED just works as an indicator that the 1Sheeld is working fine
pinMode(ledPin, OUTPUT);
myMotor->setSpeed(150);
myOtherMotor->setSpeed(150);
myMotor3->setSpeed(150);
myOtherMotor4->setSpeed(150); 
  OrientationSensor.setOnValueChange(readAngleXi);

  myservoX.attach(9);    // Servo2 (in the Motor Shield) of Axix X (PAN)  - connected to Analogic Pin 09 of Arduino
  myservoY.attach(10);   // Servo1 (in the Motor Shield) of Axix Y (TILT) - connected to Analogic Pin 10 of Arduino

}


void loop()
{

  angleX = -OrientationSensor.getX();
  angleX = angleX - angleXi + 90;
  if (angleX < 0) {
    angleX = angleX + 360;
  }
  if (angleX > 360) {
    angleX = angleX - 360;
  }
  angleX = constrain(angleX, 0, 180);

  myservoX.write(angleX);
  delay(15);

  angleY = -OrientationSensor.getY();
  angleY = 180 - (angleY - angleYi);
  angleY = constrain(angleY, -30, 150);
  myservoY.write(angleY);
  delay(15);
/* Always check the status of gamepad buttons. */
if (GamePad.isDownPressed())
{
myMotor->run(BACKWARD);
myOtherMotor->run(BACKWARD);
myMotor3->run(BACKWARD);
myOtherMotor4->run(BACKWARD);

digitalWrite(ledPin, HIGH);
}
else if (GamePad.isUpPressed())
{
myMotor->run(FORWARD);
myOtherMotor->run(FORWARD);
myMotor3->run(FORWARD);
myOtherMotor4->run(FORWARD);

digitalWrite(ledPin, HIGH);
}
else if (GamePad.isLeftPressed())
{
myMotor->run(BACKWARD);
myOtherMotor->run(BACKWARD);
myMotor3->run(FORWARD);
myOtherMotor4->run(FORWARD);

digitalWrite(ledPin, HIGH);
}
else if (GamePad.isRightPressed())
{
myMotor->run(FORWARD);
myOtherMotor->run(FORWARD);
myMotor3->run(BACKWARD);
myOtherMotor4->run(BACKWARD);

digitalWrite(ledPin, HIGH);
}
else
{
myMotor->run(RELEASE);
myOtherMotor->run(RELEASE);
myMotor3->run(RELEASE);
myOtherMotor4->run(RELEASE);

digitalWrite(ledPin, LOW);
}

}

void readAngleXi(float xAxis, float yAxis, float zAxis)
{
  if (k) {
    angleXi = -xAxis;
    k = false;
  }

}
void loop2()
{

}