path: root/Venus_Skeleton/Venus_Skeleton.ino

// TODO
//
// - limit sendData to x per second
//
// - insert into calibration files
// 		
// 
// - Make sure ENUM piDataType is correct and synced
// - Change interpretData to resemble correct ENUM data
// 

#include <Servo.h>

// **********************
// ** Calibration file **
// **********************

// Include Wally of Eve, (un)comment the right one.
#include "calibration.wall-e.h"
//#include "calibration.eve.h"


// *****************
// ** DEFINITIONS **
// *****************

// Pin configuration
#define PIN_SENS_COMPASS
#define PIN_SENS_OBSTACLE_TURRET 9
#define PIN_SENS_IR_LEFT A1
#define PIN_SENS_IR_RIGHT A0
#define PIN_SENS_SAMPLE_TURRET
#define PIN_SENS_SAMPLE_GRIPPER
#define PIN_SENS_BEACON_TURRET
#define PIN_SERVO_LEFT 12
#define PIN_SERVO_RIGHT = 13;
#define PIN_SERVO_TURRET = 11;

// Servos
Servo servoLeft;	// Declare left servo signal
Servo servoRight;	// Declare right servo signal


// Raspberry Pi Data type declarations
typedef enum {
	PI_DATATYPE_COMPASS, 
	PI_DATATYPE_OBSTACLETURRET, 
	PI_DATATYPE_IRLEFT, 
	PI_DATATYPE_IRRIGHT, 
	PI_DATATYPE_SAMPLETURRET, 
	PI_DATATYPE_SAMPLEGRIPPER, 
	PI_DATATYPE_BEACONTURRET, 
	PI_DATATYPE_LAB, 
	PI_DATATYPE_MOVETO, 
	PI_DATATYPE_GRIPPER, 
	PI_DATATYPE_TURRET
} pi_datatype_t;
// Operation modes
typedef enum {
	OPMODE_RANDOM, 
	OPMODE_WAIT, 
	OPMODE_INITIALSEQUENCE, 
	OPMODE_RANDOM, 
	OPMODE_MAPPING, 
	OPMODE_GOTOSAMPLE, 
	OPMODE_GRABSAMPLE, 
	OPMODE_GOTOLABLOCATION, 
	OPMODE_WAITFORLAB, 
	OPMODE_FINDMAGNET, 
	OPMODE_LABSEQUENCE
} opmode_t;

// Current operation mode
opmode_t operationMode = OPMODE_WAIT;
unsigned long operationChange = 0;

// Location values
int currValRobotX;
int currValRobotY;

// Sensor values
int currValSensCompass;
int currValSensObstacleTurret;
int currValServoTurret;

// Actuator values
int currValTurret;


// **********************
// ** PI COMMUNICATION **
// **********************

// sendData to Raspberry Pi
void sendData(pi_datatype_t method, int data, int data1 = null, int data2 = null) {
  
}

// receive data from Raspberry Pi
// sequence = ... ?????
void readData() {
  
	// if data received -> interpretate it
	// change operation mode + update timer
	operationMode = OPMODE_WAIT;
	operationChange = millis();
	// for example:
	interpretData(PI_DATATYPE_MOVETO, 0);
}

void interpretData(pi_datatype_t dataType, int message) {
  switch(dataType) {
    case PI_DATATYPE_MOVETO:
		
    break;
    case PI_DATATYPE_GRIPPER:
    
    break;
    case PI_DATATYPE_TURRET:
    
    break;
    case default:
		// ignore ????
    break;
  }
}


// **********************
// ** SENSOR FUNCTIONS **
// **********************

// sensor Compass, orientation
int sensCompass() {
	// average for last x ????????
  int degree = 359;
  sendData(PI_DATATYPE_COMPASS, degree);
  return degree;
}

// sensor Ultrasound, distance measurement
int sensObstacleTurret() {
  int distance = 100;
  int turretAngle = 90;
  sendData(PI_DATATYPE_OBSTACLETURRET, distance, turretAngle);
  return distance;
}

// sensor IR, line detection
boolean sensIRLeft() {
  int i;
	boolean inaccessible = false;
  i = analogRead(PIN_SENS_IR_LEFT);
  if (i > CAL_IR_LEFT_THRESHOLD)
    inaccessible = true;
	
  // Inaccessible terrain. True for inaccessible, false for accessible
  if(inaccessible) {
    stopMovement();
    sendData(PI_DATATYPE_IRLEFT, currValRobotX, currValRobotY, currValSensCompass);
  }
  return inaccessible;
}
int sensIRRight() {
  int i;
	boolean inaccessible = false;
  i = analogRead(PIN_SENS_IR_RIGHT);
  if (i > CAL_IR_RIGHT_THRESHOLD)
    inaccessible = true;
	
  // Inaccessible terrain. True for inaccessible, false for accessible
  if(inaccessible) {
    stopMovement();
    sendData(PI_DATATYPE_IRRIGHT, currValRobotX, currValRobotY, currValSensCompass);
  }
  return inaccessible;
}

// sensor IR, sample detection
int sensSampleTurret() {
  boolean sample = false;
  if(sample) {
		// If a sample is detected, stop all servos, send data to the PI and wait for response
    stopAllServos();
    sendData(PI_DATATYPE_SAMPLETURRET, currValRobotX, currValRobotY, currValSensCompass, currValTurret);
  }
}
int sensSampleGripper() {
  boolean sample = false;
  if(sample) {
    stopAllServos();
    sendData(PI_DATATYPE_SAMPLEGRIPPER, currValRobotX, currValRobotY, currValSensCompass);
  }
}

// sensor IR, beacon detection and recognition
int sensBeaconTurret() {
  int beacon = 0;
  if(beacon) {
    // Location
  }
}

// sensor IR, lab detection
int sensLab() {
  return 0;
}

// ************************
// ** ACTUATOR FUNCTIONS **
// ************************

void stopMovement() {
	operationMode = OPMODE_WAIT;
	servoLeft.detach();
	servoRight.detach();
}
void stopAllServos() {
	operationMode = OPMODE_WAIT;
	servoLeft.detach();
	servoRight.detach();
}

// ***********************
// ** ARDUINO FUNCTIONS **
// ***********************

void setup() {
  
}

void loop() {
	sensCompass();
	sensObstacleTurret();
	sensIRLeft();
	sensIRRight();
	sensSampleTurret();
	sensSampleGripper();
	sensBeaconTurret();
	sensLab();
	
	switch(operationMode) {
		case OPMODE_WAIT:
		stopMovement();
		break;
		
		default:
		stopMovement();
		break;
	}
	
}

// **********************
// ** HELPER FUNCTIONS **
// **********************

long microsecondsToInches(long microseconds)
{
  // According to Parallax's datasheet for the PING))), there are
  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
  // second).  This gives the distance travelled by the ping, outbound
  // and return, so we divide by 2 to get the distance of the obstacle.
  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
  return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}

















// define types
typedef struct {
	// ir
	bool left_detected;
	bool right_detected; // usually an integer (16-bit)
	int distance; // 16-bit
	// ..
	long timer; // 32-bit
} sensor_data_t;

enum {
	CHANGED_IR_LEFT = 1 << 0,
	CHANGED_IR_RIGHT = 1 << 1,
	CHNAGED_JDHCHDUCDH = 1 << 2,
		CHANGED_IDJDHD = 1 << 3
};

sensor_data_t data;
int changedBits = 0;

// 1. gather sensor data -< sets changedBits and data
val = readFromSensor();
if (data.left_detected != val) {
	data.left_detected = val;
	// mark as changed
	changedBits |= CHANGED_IR_LEFT;
}
// other sensors: same

// 2. read whetehr you have hit a block / line sensor
if ((changedBits & (CHANGED_IR_LEFT | CHANGED_IR_RIGHT))) &&
	(data.left_detected || data.right_detected)) {
		// panic!
		stopMotor();
}

// 3. send to pi or other calculations (and clear bits)
sendData(PIN_IR_LEFT, data.left_detected);
// clear bits
changedBits &= ~CHANGED_IR_LEFT;