path: root/Venus_Skeleton/Venus_Skeleton.ino

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

// *****************
// ** ERROR CODES **
// *****************
// 
// 0		no error
// 1		Error: Sensor Compass error
// 2		Error: Sensor Obstacle detection on the turret
// 3		Error: Sensor IR Line detection

#include <Servo.h>
#include "dataTypes.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;
Servo servoRight;
Servo servoTurret;

// Data variables
data_t data;
int dataToPiChangedBits = 0;

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

// Location values
int currValRobotX = 0;
int currValRobotY = 0;
int LabX = 0;
int LabY = 0;

// Number of directions the robot can choose from: ((x-1) * (360/x)) degree. Preferably a multiple of 4, I think.
#define NUM_DIRECTIONS 12

// Number of directions the turret can measure from
#define NUM_TURRET_DIRECTIONS 13
int turretSequence[NUM_TURRET_DIRECTIONS] = {};

// Sensor values
int currValSensCompass = 0;
int currValSensObstacleTurretLow = 0;
int currValServoTurret = 0;

// Actuator values
int currValTurret = 0;
int currValDirection = 0;

// Timers
long timerMovement = 0;
long timerInitialSequence = 0;
long timerTurret = 0;

// Counters
int counterMovement = 0;
int counterTurret = -1;


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

// error helper
void error(int errorCode) {
	opMode(OPMODE_ERROR);
	stopAllServos();
	errorSequence(errorCode);
}
void errorSequence(int errorCode) {
	while(1) {
		// - move turret to middle (fast)
		// - move turret to left (slow)
		// - move turret to middle (slow)
		delay(1000);
		for(int i = 0; i < errorCode; i++) {
			// - move turret to right (fast)
			// - move turret to middle (fast)
			delay(500);
		}
		delay(3000);
	}
}

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;
}

void initTurretSequence(int array, int numOfPositions) {
	int sign = 1;
	int x = 0;

	for (int i = 0; i < numOfPositions; i++) {
		array[i] = x;
		x += sign * 2;

		if(x > numOfPositions) {
			sign = -1;
			x -= 3;
		}
		if(x == numOfPositions) {
			sign = -1;
			x -= 1;
		}
	}
}



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

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

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;
    default:
		// ignore ????
    break;
  }
}

// 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);
}


// **********************
// ** 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
long sensPing(int pin)
{
  long duration;
	
	// Sets pin to OUTPUT and send start signal: pulse (Low-High-Low for clean signal)
  pinMode(pin, OUTPUT);
  digitalWrite(pin, 0);
  delayMicroseconds(2);
  digitalWrite(pin, 255);
  delayMicroseconds(5);
  digitalWrite(pin, 0);
	
	// Sets pin to INPUT and measure time from the echo
  pinMode(pin, INPUT);
  duration = pulseIn(pin, 255);
	
	// Convert from us to cm
  duration = microsecondsToCentimeters(duration);
  return duration;
}
void sensObstacleTurret() {
	
	// BE AWARE, LOCALISATION OF OBSTACLE IS y DISTANCE FROM CENTER OF ROBOT 
	// (creates triangle with centerOfRobot, centerOfSensor and Obstacle as its corners)
	
	if (timerTurret == 0) {
		long distance = sensPing(PIN_SENS_OBSTACLE_TURRET);
		if (data.obstacle_turret_distances[counterTurret] != distance) {
			data.obstacle_turret_distances[counterTurret] = distance;
			// mark as changed
			dataToPiChangedBits |= CHANGED_SENS_OBSTACLETURRET;
		}
  }
}


// sensor IR, line detection
bool sensIRLine(lr_t LorR) {
	
	
	// BE AWARE, LOCALISATION OF LINE IS y DISTANCE and z DEGREE FROM CENTER OF ROBOT,
	// calculate by RPi??
	
	
  int i;
	bool inaccessible = false;
	int pinNo = LorR == L ? PIN_SENS_IR_LEFT : PIN_SENS_IR_RIGHT;
	int threshold = LorR == L ? CAL_IR_LEFT_THRESHOLD : CAL_IR_RIGHT_THRESHOLD;
	bool *dataField = LorR == L ? &data.IR_left_detected : &data.IR_right_detected;
	int changeBit = LorR == L ? CHANGED_SENS_IRLEFT : CHANGED_SENS_IRRIGHT;
  
  i = analogRead(pinNo);
  if (i > threshold)
    inaccessible = true;
	
	if (*dataField != inaccessible) {
		*dataField = inaccessible;
		// mark as changed
		dataToPiChangedBits |= changeBit;
	}
	
 	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
		opMode(OPMODE_WAIT);
    stopAllServos();
    sendData(PI_DATATYPE_SAMPLETURRET, currValRobotX, currValRobotY, currValSensCompass, currValTurret);
  }
}
int sensSampleGripper() {
  int i;
	bool sample = false;
	
	i = false; // sensor readout here
	if (i > CAL_SAMPLE_GRIPPER_THRESHOLD) 
		sample = true;
	
	if(data.sample_gripper_detected != sample) {
		data.sample_gripper_detected = sample;
		// Mark as changed
		dataToPiChangedBits |= CHANGED_SENS_SAMPLEGRIPPER;
	}
	
  return sample;
}

// 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() {
	servoLeft.detach();
	servoRight.detach();
}

void stopAllServos() {
	servoLeft.detach();
	servoRight.detach();
	
	timerTurret == 0;
}

bool moveTurnTo(int direction) {
	if (direction == 0 || direction == NUM_DIRECTIONS) {
		return true;
	}
	
	// if movementtimer is not started, start timer or something and initiate movement
	// else continue movement and return false
	// if movementtimer is expired, stop movement.
	
	if (!timerMovement) {
		counterMovement++;
		// timer = now() + direction * formula
		timerMovement = millis() + (direction % NUM_DIRECTIONS) * 200;
		
		
		// DO A COMPASS CHECK
		// movePivot('R', somespeed);
		
		
	}
	if (timerMovement < millis()) {
		stopMovement();
		currValDirection = 0;
	}
}

void moveStraight(int direction, int distance) {
	if (direction != 0 && direction != NUM_DIRECTIONS) {
		return;
	}
	
	
}

void turnTurretTo(int deg) {
	deg = deg + CAL_TURRET_DEGREE;
	servoTurret.write(deg);
}
void turnTurretToNext() {
	if(timerTurret == 0) {
		counterTurret++;
		if(counterTurret >= NUM_TURRET_DIRECTIONS) {
			counterTurret == 0;
		}
		
		int deg;
	  deg = turretSequence[counterTurret] * 10;
		
		timerTurret = millis();
	  turnTurretTo(deg)
	}
	if ((timerTurret + 100) < millis()) {
		timerTurret = 0;
	}
}


// *********************
// ** LOGIC FUNCTIONS **
// *********************

void opMode(opmode_t opmode) {
	operationMode = opmode;
}

void checkFreePath() {
	if ((dataToPiChangedBits & (CHANGED_SENS_IRLEFT | CHANGED_SENS_IRRIGHT)) && (data.IR_left_detected || data.IR_right_detected)) {
		// Left or Right IR sensor sees inaccessible terrain
		opMode(OPMODE_WAIT);
		stopMovement();
	}
	
	// Ultrasound
	
}

int checkBestRoute() {
	// possibly use time instead of boolean values, to remember them, default value = default time, possible to let pi change this
	// possibly change directionArray to global variable
	if (counterMovement) {
		return 0;
	}
	
	// Array with all directions (for example with 12 directions):
	// Bottom half: ([00-11] % 12) * (360/12) degrees to the left
	// Top half: 		([12-23] % 12) * (360/12) degrees to the right
	int directionArray[NUM_DIRECTIONS * 2] = {};
	
	if (!eliminateDirections(directionArray)) {
		// We are screwed!!! Basically robot is dead or trapped.
		return false;
	}
	
	int direction = 0;
	do {
		direction = micros() % (NUM_DIRECTIONS * 2);
	} while (!directionArray[direction]);
	
	currValDirection = direction;
	return direction;
}

int eliminateDirections(int directionArray[]) {
	int numPossibleDirections = 0;
	// Implement PI's choices (if still possible)
	// int numPossibleDirectionsPi = 0;
	for (int i = 0; i < NUM_DIRECTIONS * 2; i++) {
		directionArray[i] = 1;
	}
	
	for (int i = 0; i < NUM_DIRECTIONS * 2; i++) {
		// LINE PANIC! If a line is detected on the left, only a right turn is allowed between 90deg and 180deg
		if (data.IR_left_detected && ((i < ((float)NUM_DIRECTIONS / 4.0) + NUM_DIRECTIONS) || (i > ((float)NUM_DIRECTIONS / 4.0) * 2.0 + NUM_DIRECTIONS)) ) {
			directionArray[i] = 0;
		}
		// LINE PANIC! If a line is detected on the right, only a left turn is allowed between 90deg and 180deg
		if (data.IR_right_detected && ( (i < ((float)NUM_DIRECTIONS / 4.0)) || (i > ((float)NUM_DIRECTIONS / 4.0)*2.0) )) {
			directionArray[i] = 0;
		}
		// ULTRASOUND PANIC! If ...
		
		
		if (directionArray[i]) {
			numPossibleDirections++;
			// Implement PI's choices (if still possible)
			// if(PIarray[i]) {numPossibleDirectionsPi++;}
		}
	}

	// Implement PI's choices (if still possible)
	// if (numPossibleDirectionsPi) {
		//for (int i = 0; i < NUM_DIRECTIONS * 2; i++) {
			// if(!PIarray[i]) {directionArray[i] = 0}
		// }
	// }
	
	return numPossibleDirections;
}

void checkSample() {
	if ((dataToPiChangedBits & (CHANGED_SENS_SAMPLETURRET | CHANGED_SENS_SAMPLEGRIPPER)) && (data.sample_turret_detected || data.sample_gripper_detected)) {
		// One of the turret sees a sample (or the lab)
		opMode(OPMODE_CHECKSAMPLE);
		stopAllServos();
		sensSampleTurret();
		sensSampleGripper();
	}
}

bool confirmSample() {
	if (data.sample_turret_detected || data.sample_gripper_detected) {
		if (!data.sample_gripper_detected) {
			// Turn robot to sample
			// If still no sample_gripper_detected return false
		}
		// What is distance
	}
}

void initialSequence() {
	moveStraight(0, CAL_INITIAL_DISTANCE);
}

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

void setup() {
	// initial communication between Arduino and RPi
	// Send some values:
	// - Number of possible directions
	
	initTurretSequence(turretSequence, NUM_TURRET_DIRECTIONS);
	
	servoLeft.attach(servoLeftPin);
	servoRight.attach(servoRightPin);
	servoTurret.attach(servoTurretPin);
	
	turnTurretTo(0);
	
	delay(100); // Make sure all actuators are at their starting position
}

void loop() {
	sensCompass();
	sensObstacleTurret();
	sensIRLine(L);
	sensIRLine(R);
	sensSampleTurret();
	sensSampleGripper();
	
	sensLab();

	checkSample();
	checkFreePath();
	
	// calculateLocation();
	
	sendData(PI_DATATYPE_ALL, 'lala', 'lala');
	dataToPiChangedBits = 0; // Clear changed bits
	readData();
	
	switch(operationMode) {
		case OPMODE_WAIT:
			counterMovement = 0;
			timerMovement = 0;
			break;
		case OPMODE_INITIALSEQUENCE:
			initialSequence();
			break;
		case OPMODE_RANDOM:
			
			break;
		case OPMODE_MAPPING:
			turnTurretToNext();
			checkBestRoute();
			moveTurnTo(currValDirection);
			moveStraight(currValDirection, 1); // 1 is supposed to be some distance / time
			break;
		case OPMODE_CHECKSAMPLE:
			
			break;
		case OPMODE_GRABSAMPLE:
			
			break;
		case OPMODE_GOTOLABLOCATION:
			sensBeaconTurret();
			break;
		case OPMODE_WAITFORLAB:
			
			break;
		case OPMODE_FINDMAGNET:
			
			break;
		case OPMODE_LABSEQUENCE:
			
			break;
		default:
			// ??
			break;
	}
	
	// Delay for stability.
	delay(20);
}

















// // 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;dataTypes.h