From a40c1d26734ff1940ad125fd9b10aa4dfd145f1d Mon Sep 17 00:00:00 2001 From: Peter de Kok Date: Wed, 10 Jun 2015 11:29:28 +0200 Subject: some changes again --- Venus_Skeleton/Venus_Skeleton.ino | 163 ++++++++++++++++++++++++------------ Venus_Skeleton/calibration_wall-e.h | 13 ++- Venus_Skeleton/dataTypes.h | 4 +- 3 files changed, 123 insertions(+), 57 deletions(-) diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino index 60df676..c51e4ba 100644 --- a/Venus_Skeleton/Venus_Skeleton.ino +++ b/Venus_Skeleton/Venus_Skeleton.ino @@ -63,7 +63,7 @@ unsigned long lastCommunication = 0; // Current operation mode opmode_t operationMode = OPMODE_WAIT; -opmode_t operationModeDefault = OPMODE_WAIT; +opmode_t operationModeDefault = OPMODE_INITIALSEQUENCE; unsigned long operationChange = 0; // Location values @@ -85,11 +85,13 @@ int currValTurret = 0; int currValDirection = 0; // Timers -long timerMovement = 0; +long timerMovementStart = 0; +long timerMovementStop = 0; long timerInitialSequence = 0; long timerTurret = 0; // Counters +int counterInit = 0; int counterMovement = 0; int counterTurret = -1; int counterTurretWait = 0; @@ -208,12 +210,10 @@ bool checkIfPiAlive() { } bool checkRPiActive() { - RPiActive = ((lastCommunication + MAX_COMMUNICATION_DELAY) > millis()); - + RPiActive = (lastCommunication + MAX_COMMUNICATION_DELAY) > millis() ? 1 : 0; if(!RPiActive) { RPiActive = checkIfPiAlive(); } - return RPiActive; } @@ -408,14 +408,16 @@ void maneuver(int speedLeft, int speedRight){ void stopMovement() { DEBUG_PRINT(" Stop Movement\n"); maneuver(0, 0); - timerMovement = 0; + timerMovementStart = 0; + timerMovementStop = 0; } void stopAllServos() { DEBUG_PRINT(" Stop All Servos\n"); maneuver(0, 0); timerTurret = 0; - timerMovement = 0; + timerMovementStart = 0; + timerMovementStop = 0; } bool moveTurnTo(int direction) { @@ -423,35 +425,48 @@ bool moveTurnTo(int direction) { return true; } + // Make sure turret doesn't move + timerTurret = 1; + // 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) { - DEBUG_PRINT("\n Start turn move: "); + if (!timerMovementStop) { + DEBUG_PRINT(" Start turn move: "); DEBUG_PRINT(direction); DEBUG_PRINT(" - "); counterMovement++; // timer = now() + direction * formula - timerMovement = millis() + (direction % NUM_DIRECTIONS) * CAL_MOVE_TURN * (360 / NUM_DIRECTIONS); + timerMovementStart = millis(); + timerMovementStop = millis() + (direction % NUM_DIRECTIONS) * CAL_MOVE_TURN * (360 / NUM_DIRECTIONS); // DO A COMPASS CHECK maneuver(0,0); if(direction < NUM_DIRECTIONS){ - DEBUG_PRINT(" left\n\n"); + DEBUG_PRINT(" left\n"); // turn left + int sign = -1; maneuver(-100,100); } if(direction >= NUM_DIRECTIONS){ - DEBUG_PRINT(" right\n\n"); + DEBUG_PRINT(" right\n"); // Turn right + int sign = 1; maneuver(100,-100); } + } else { + int deg = ((int)((millis() - timerMovementStart) * CAL_MOVE_TURN * sign)) % 360; + data.robot_curr_deg = (data.robot_curr_deg + deg + 360*3) % 360; } - if (timerMovement < millis()) { + + if (timerMovementStop < millis()) { stopMovement(); currValDirection = 0; + for (int k = 0; k < NUM_TURRET_DIRECTIONS; k++) { + data.obstacle_turret_distances[k] = 300; + } } } @@ -461,32 +476,38 @@ void moveStraight(int direction, float distance) { } // Convert distance into drive time - float drivetime = distance / 0.0154; + float drivetime = distance / CAL_MOVE_STRAIGHT; - if (!timerMovement) { - DEBUG_PRINT("\n Start straight move: "); + if (!timerMovementStop) { + DEBUG_PRINT(" Start straight move: "); DEBUG_PRINT(distance); DEBUG_PRINT(" - "); counterMovement++; // timer = now() + direction * formula - timerMovement = millis() + drivetime; + timerMovementStart = millis(); + timerMovementStop = millis() + drivetime; // If input is positive drive forward if(distance > 0) { - DEBUG_PRINT(" fw\n\n"); + DEBUG_PRINT(" fw\n"); maneuver(100, 100); } // If input is negative drive backwards if(distance < 0){ - DEBUG_PRINT(" bw\n\n"); + DEBUG_PRINT(" bw\n"); maneuver(-100, -100); } + } else { + float dist = (millis() - timerMovementStart) * CAL_MOVE_STRAIGHT; + data.robot_curr_x += dist * sin((data.robot_curr_deg/180.0)*PI); + data.robot_curr_y += dist * cos((data.robot_curr_deg/180.0)*PI); } - if (timerMovement < millis()) { + if (timerMovementStop < millis()) { stopMovement(); counterMovement = 0; } + } void turnTurretTo(int deg) { @@ -496,9 +517,7 @@ void turnTurretTo(int deg) { void turnTurretToNext() { if(timerTurret == 0) { counterTurret++; - DEBUG_PRINT("Turret: "); - DEBUG_PRINT(counterTurret); - DEBUG_PRINT("\n"); + counterTurretWait++; if(counterTurret >= NUM_TURRET_DIRECTIONS) { counterTurret = 0; } @@ -524,6 +543,7 @@ void opMode(opmode_t opmode) { DEBUG_PRINT(opmode); DEBUG_PRINT("\n\n"); operationMode = opmode; + counterTurretWait = 0; } void checkFreePath() { @@ -539,9 +559,9 @@ void checkFreePath() { } // Ultrasound - if ((dataToPiChangedBits & CHANGED_SENS_OBSTACLETURRET) && - (counterTurret >= (NUM_TURRET_DIRECTIONS / 4)) && - (counterTurret < (NUM_TURRET_DIRECTIONS - (NUM_TURRET_DIRECTIONS / 4))) && + if (// (dataToPiChangedBits & CHANGED_SENS_OBSTACLETURRET) && + (counterTurret >= (NUM_TURRET_DIRECTIONS / 6)) && + (counterTurret < (NUM_TURRET_DIRECTIONS - (NUM_TURRET_DIRECTIONS / 6))) && (data.obstacle_turret_distances[counterTurret] < OBSTACLE_TOO_CLOSE)) { // Ultrasound sensor sees inaccessible terrain opMode(OPMODE_WAIT); @@ -563,22 +583,44 @@ int checkBestRoute(int preferedDirection) { // 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] = {}; - for (int i = 0; i < NUM_DIRECTIONS * 2; i++) { + for (int i = 0; i < (NUM_DIRECTIONS * 2); i++) { if (preferedDirection == -1) { directionArray[i] = 1; } else { - directionArray[i] = 10 + abs((int)(10 * (float)(abs(((NUM_DIRECTIONS - abs(i - NUM_DIRECTIONS)) % NUM_DIRECTIONS) - preferedDirection) - (float)NUM_DIRECTIONS / 2))); + directionArray[i] = 100 + abs((int)(100 * (float)(abs(((NUM_DIRECTIONS - abs(i - NUM_DIRECTIONS)) % NUM_DIRECTIONS) - preferedDirection) - (float)NUM_DIRECTIONS / 2))); + } + + // limit to first 180deg + if( (i % NUM_DIRECTIONS) > (NUM_DIRECTIONS / 2) ) { + directionArray[i] = 0; } } if (!eliminateDirections(directionArray)) { // We are screwed!!! Basically robot is dead or trapped. return false; + while(1) { + delay(1000); + } } int direction = 0; + int sumDirectionPriority = 0; + int randValue = 0; + + for (int i = 0; i < (NUM_DIRECTIONS * 2); i++) { + sumDirectionPriority += directionArray[i]; + } + do { - direction = TrueRandom.random(NUM_DIRECTIONS * 2); + randValue = TrueRandom.random(sumDirectionPriority); + for (int i = 0; i < (NUM_DIRECTIONS * 2); i++) { + randValue -= directionArray[i]; + if (randValue < 0) { + direction = i--; + break; + } + } } while (!directionArray[direction]); currValDirection = direction; @@ -654,7 +696,19 @@ bool confirmSample() { } void initialSequence() { - moveStraight(0, CAL_INITIAL_DISTANCE); + do { + moveStraight(0, CAL_INITIAL_DISTANCE1); + } while(timerMovementStop); + // Set x and y value of robot to (0, 0) for lab/starting location. + data.robot_curr_x = 0; + data.robot_curr_y = 0; + do { + moveStraight(0, CAL_INITIAL_DISTANCE2); + } while(timerMovementStop); +} + +void calculateOrientation() { + // current + compass } // *********************** @@ -702,15 +756,13 @@ void setup() { turnTurretTo(0); - lastCommunication = millis(); - delay(100); // Make sure all actuators are at their starting position } void loop() { //sensCompass(); - //sensObstacleTurret(); + sensObstacleTurret(); sensIRLine(L); sensIRLine(R); //sensSampleTurret(); @@ -720,60 +772,65 @@ void loop() { //checkSample(); checkFreePath(); - - // calculateLocation(); + + // calculateOrientation(); + // calculateLocationOffset(); if (checkRPiActive()) { + operationModeDefault = (operationModeDefault == OPMODE_INITIALSEQUENCE ? OPMODE_INITIALSEQUENCE : OPMODE_WAIT); //sendData(PI_DATATYPE_ALL, 'lala', 'lala'); //readData(); } else { // STANDALONE ALGORITHM + operationModeDefault = (operationModeDefault == OPMODE_INITIALSEQUENCE ? OPMODE_INITIALSEQUENCE : OPMODE_MAPPING); } - dataToPiChangedBits = 0; // Clear changed bits + dataToPiChangedBits = 0; // Clear changed bits + switch(operationMode) { case OPMODE_WAIT: counterMovement = 0; - timerMovement = 0; - - // Make sure obstacle_turret_distances array is filled with new data before moving on - if (counterTurretWait == NUM_TURRET_DIRECTIONS) { - opMode(operationModeDefault); - } - + timerMovementStart = 0; + timerMovementStop = 0; + opMode(operationModeDefault); + turnTurretToNext(); break; + case OPMODE_INITIALSEQUENCE: initialSequence(); + operationModeDefault = OPMODE_WAIT; break; + case OPMODE_RANDOM: - break; + case OPMODE_MAPPING: turnTurretToNext(); - checkBestRoute(-1); + checkBestRoute(0); moveTurnTo(currValDirection); - moveStraight(currValDirection, 20.0); // 1.0 is supposed to be some distance + moveStraight(currValDirection, 30.0); break; + case OPMODE_CHECKSAMPLE: - break; - case OPMODE_GRABSAMPLE: - + + case OPMODE_GRABSAMPLE: break; + case OPMODE_GOTOLABLOCATION: sensBeaconTurret(); break; + case OPMODE_WAITFORLAB: - break; + case OPMODE_FINDMAGNET: - break; + case OPMODE_LABSEQUENCE: - break; + default: - // ?? break; } diff --git a/Venus_Skeleton/calibration_wall-e.h b/Venus_Skeleton/calibration_wall-e.h index 61848f3..2c88c38 100644 --- a/Venus_Skeleton/calibration_wall-e.h +++ b/Venus_Skeleton/calibration_wall-e.h @@ -6,13 +6,22 @@ #define CAL_IR_RIGHT_THRESHOLD 600 #define CAL_SAMPLE_GRIPPER_THRESHOLD 1 #define CAL_SAMPLE_TURRET_THRESHOLD 1 -#define CAL_INITIAL_DISTANCE 30.0 -#define CAL_TURRET_DEGREE 7 + +// Distance to drive during initial sequence. (to (x, y) (0, 0)) +#define CAL_INITIAL_DISTANCE1 60.0 +// Distance to drive during initial sequence (to starting position - one robot further then the other, to prevent collissions) +#define CAL_INITIAL_DISTANCE2 20.0 + +#define CAL_TURRET_DEGREE 10 // Left wheel calibration factor WALL-E: 1.4 EVE: 1.0 #define CAL_SERVO_LEFT 1.4 // Right wheel calibration factor WALL-E: 1.0 EVE: 1.1 #define CAL_SERVO_RIGHT 1.0 +// Movement calibration +// Turn calibration (ms per degree) #define CAL_MOVE_TURN 6.05 +// Straight calibration ((1/ms) per cm) +#define CAL_MOVE_TURN 0.0154 diff --git a/Venus_Skeleton/dataTypes.h b/Venus_Skeleton/dataTypes.h index 9021dc6..f19eaf5 100644 --- a/Venus_Skeleton/dataTypes.h +++ b/Venus_Skeleton/dataTypes.h @@ -20,8 +20,8 @@ typedef struct { int robot_x[NUM_TURRET_DIRECTIONS]; int robot_y[NUM_TURRET_DIRECTIONS]; int robot_deg[NUM_TURRET_DIRECTIONS]; - int robot_curr_x; - int robot_curr_y; + float robot_curr_x; + float robot_curr_y; int robot_curr_deg; } data_t; enum { -- cgit v1.2.1