From 4c5dbd6087f49a1dd4f80303d099d61ee4faaa6f Mon Sep 17 00:00:00 2001 From: Peter de Kok Date: Wed, 3 Jun 2015 17:18:04 +0200 Subject: Testing now --- Venus_Skeleton/Venus_Skeleton.ino | 161 ++++++++++++++++++++++++------------ Venus_Skeleton/calibration_wall-e.h | 9 +- Venus_Skeleton/dataTypes.h | 1 + 3 files changed, 118 insertions(+), 53 deletions(-) diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino index e07a2b4..d784255 100644 --- a/Venus_Skeleton/Venus_Skeleton.ino +++ b/Venus_Skeleton/Venus_Skeleton.ino @@ -16,6 +16,14 @@ // 2 Error: Sensor Obstacle detection on the turret // 3 Error: Sensor IR Line detection +#define DEBUG true + +#ifdef DEBUG +#define DEBUG_PRINT(str) Serial.print(str) +#else +#define DEBUG_PRINT(str) do { } while (0) +#endif + #include #include #include @@ -28,6 +36,7 @@ #include "dataTypes.h" // Include Wally or Eve, (un)comment the right one. +#define ROVER "wall-e" #include "calibration_wall-e.h" //#include "calibration.eve.h" @@ -79,6 +88,7 @@ long timerTurret = 0; // Counters int counterMovement = 0; int counterTurret = -1; +int initCounter = 0; // ********************** @@ -124,7 +134,7 @@ long microsecondsToCentimeters(long microseconds) return microseconds / 29 / 2; } -void initTurretSequence(int array, int numOfPositions) { +void initTurretSequence(int array[], int numOfPositions) { int sign = 1; int x = 0; @@ -150,7 +160,7 @@ void initTurretSequence(int array, int numOfPositions) { // ********************** // sendData to Raspberry Pi -void sendData(pi_datatype_t method, int data, int data1 = NULL, int data2 = NULL, int data3 = NULL) { +void sendData(pi_datatype_t method, int data, int data1, int data2, int data3) { } @@ -214,36 +224,25 @@ float sensMagnetometer() { } return(north); // Returns angle difference } -int sensCompass() { - - +void sensCompass() { + int degree = -1; float reading[NUM_COMPASS_CHECKS]; float readingtotal = 0; for(int i = 0; i < NUM_COMPASS_CHECKS; i++){ - reading[i] = readcompass(); - readingtotal - i++; - reading[i] = readcompass(); - i++; - reading[i] = readcompass(); - } - if(i==NUM_COMPASS_CHECKS){ - for(int j = 0; j < NUM_COMPASS_CHECKS; j++){ - readingtotal = readingtotal + reading[j]; // Total readings - } - readingtotal = readingtotal / NUM_COMPASS_CHECKS; // Average reading - Serial.print("Readingtotal: "); // Print average reading (CAN BE REMOVED) - Serial.println(readingtotal); + readingtotal += sensMagnetometer(); } - return readingtotal; - + readingtotal /= NUM_COMPASS_CHECKS; + + if (readingtotal >= 0 && readingtotal <= 360) + degree = (int) readingtotal; - // average for last x ???????? - int degree = 359; - sendData(PI_DATATYPE_COMPASS, degree); - return degree; + if (data.compass != degree) { + data.compass = degree; + // mark as changed + dataToPiChangedBits |= CHANGED_SENS_COMPASS; + } } // sensor Ultrasound, distance measurement @@ -284,7 +283,7 @@ void sensObstacleTurret() { // sensor IR, line detection -bool sensIRLine(lr_t LorR) { +void sensIRLine(lr_t LorR) { // BE AWARE, LOCALISATION OF LINE IS y DISTANCE and z DEGREE FROM CENTER OF ROBOT, // calculate by RPi?? @@ -305,8 +304,6 @@ bool sensIRLine(lr_t LorR) { // mark as changed dataToPiChangedBits |= changeBit; } - - return inaccessible; } // sensor IR, sample detection @@ -360,8 +357,8 @@ int sensBeaconTurret() { if (i > CAL_SAMPLE_TURRET_THRESHOLD) beacon = true; - if (data.beacon_detected[counterTurret] != sample) { - data.beacon_detected[counterTurret] = sample; + if (data.beacon_detected[counterTurret] != beacon) { + data.beacon_detected[counterTurret] = beacon; // mark as changed dataToPiChangedBits |= CHANGED_SENS_SAMPLETURRET; } @@ -369,23 +366,31 @@ int sensBeaconTurret() { } // sensor IR, lab detection -int sensLab() { - return 0; +void sensLab() { + // ... } // ************************ // ** ACTUATOR FUNCTIONS ** // ************************ +void maneuver(int speedLeft, int speedRight){ + speedLeft *= CAL_SERVO_LEFT; + speedRight *= CAL_SERVO_RIGHT; + + servoLeft.writeMicroseconds(1500 + speedLeft); // Set left servo speed + servoRight.writeMicroseconds(1500 - speedRight); // Set right servo speed +} + void stopMovement() { - servoLeft.detach(); - servoRight.detach(); + DEBUG_PRINT(" Stop Movement\n"); + maneuver(0, 0); + timerTurret == 0; } void stopAllServos() { - servoLeft.detach(); - servoRight.detach(); - + DEBUG_PRINT(" Stop All Servos\n"); + maneuver(0, 0); timerTurret == 0; } @@ -399,15 +404,26 @@ bool moveTurnTo(int direction) { // if movementtimer is expired, stop movement. if (!timerMovement) { + DEBUG_PRINT("\n Start turn move: "); + DEBUG_PRINT(direction); + DEBUG_PRINT(" - "); counterMovement++; // timer = now() + direction * formula - timerMovement = millis() + (direction % NUM_DIRECTIONS) * 200; - + timerMovement = millis() + (direction % NUM_DIRECTIONS) * CAL_MOVE_TURN * (360 / NUM_DIRECTIONS); // DO A COMPASS CHECK - // movePivot('R', somespeed); - + maneuver(0,0); + if(direction < NUM_DIRECTIONS){ + DEBUG_PRINT(" left\n\n"); + // turn left + maneuver(-100,100); + } + if(direction >= NUM_DIRECTIONS){ + DEBUG_PRINT(" right\n\n"); + // Turn right + maneuver(100,-100); + } } if (timerMovement < millis()) { stopMovement(); @@ -415,12 +431,37 @@ bool moveTurnTo(int direction) { } } -void moveStraight(int direction, int distance) { +void moveStraight(int direction, float distance) { if (direction != 0 && direction != NUM_DIRECTIONS) { return; } + // Convert distance into drive time + float drivetime = distance / 0.0154; + + if (!timerMovement) { + DEBUG_PRINT("\n Start straight move: "); + DEBUG_PRINT(distance); + DEBUG_PRINT(" - "); + counterMovement++; + // timer = now() + direction * formula + timerMovement = millis() + drivetime; + + // If input is positive drive forward + if(distance > 0) { + DEBUG_PRINT(" fw\n\n"); + maneuver(100, 100); + } + // If input is negative drive backwards + if(distance < 0){ + DEBUG_PRINT(" bw\n\n"); + maneuver(-100, -100); + } + } + if (timerMovement < millis()) { + stopMovement(); + } } void turnTurretTo(int deg) { @@ -438,7 +479,7 @@ void turnTurretToNext() { deg = turretSequence[counterTurret] * 10; timerTurret = millis(); - turnTurretTo(deg) + turnTurretTo(deg); } if ((timerTurret + 100) < millis()) { timerTurret = 0; @@ -451,6 +492,9 @@ void turnTurretToNext() { // ********************* void opMode(opmode_t opmode) { + DEBUG_PRINT("\nOPMODE"); + DEBUG_PRINT(opmode); + DEBUG_PRINT("\n\n"); operationMode = opmode; } @@ -459,6 +503,7 @@ void checkFreePath() { // Left or Right IR sensor sees inaccessible terrain opMode(OPMODE_WAIT); stopMovement(); + DEBUG_PRINT(" NO FREE PATH!! "); } // Ultrasound @@ -562,6 +607,13 @@ void setup() { // - Number of possible directions Wire.begin(); +#ifdef DEBUG + Serial.begin(9600); +#endif + + DEBUG_PRINT("\n\n\n** ROVER "); + DEBUG_PRINT(ROVER); + DEBUG_PRINT(" **\n\n"); initTurretSequence(turretSequence, NUM_TURRET_DIRECTIONS); @@ -570,17 +622,18 @@ void setup() { // Gauss scale 1.3 int error = compass.SetScale(1.3); if(error != 0){ - // Serial.println(compass.GetErrorText(error)); + DEBUG_PRINT(compass.GetErrorText(error)); + DEBUG_PRINT("\n"); } error = compass.SetMeasurementMode(Measurement_Continuous); if(error != 0){ - // Serial.println(compass.GetErrorText(error)); + DEBUG_PRINT(compass.GetErrorText(error)); } servoLeft.attach(PIN_SERVO_LEFT); servoRight.attach(PIN_SERVO_RIGHT); servoTurret.attach(PIN_SERVO_TURRET); - servoGrabber.attach(PIN_SERVO_GRIPPER); + servoGripper.attach(PIN_SERVO_GRIPPER); turnTurretTo(0); @@ -588,6 +641,7 @@ void setup() { } void loop() { + sensCompass(); sensObstacleTurret(); sensIRLine(L); @@ -596,15 +650,15 @@ void loop() { sensSampleGripper(); sensLab(); - - checkSample(); + + //checkSample(); checkFreePath(); // calculateLocation(); - sendData(PI_DATATYPE_ALL, 'lala', 'lala'); + //sendData(PI_DATATYPE_ALL, 'lala', 'lala'); dataToPiChangedBits = 0; // Clear changed bits - readData(); + //readData(); switch(operationMode) { case OPMODE_WAIT: @@ -621,7 +675,7 @@ void loop() { turnTurretToNext(); checkBestRoute(); moveTurnTo(currValDirection); - moveStraight(currValDirection, 1); // 1 is supposed to be some distance / time + moveStraight(currValDirection, 20.0); // 1.0 is supposed to be some distance break; case OPMODE_CHECKSAMPLE: @@ -646,6 +700,11 @@ void loop() { break; } + if (!initCounter) { + opMode(OPMODE_MAPPING); + initCounter++; + } + // Delay for stability. delay(20); } \ No newline at end of file diff --git a/Venus_Skeleton/calibration_wall-e.h b/Venus_Skeleton/calibration_wall-e.h index 462fa1b..daf8a8e 100644 --- a/Venus_Skeleton/calibration_wall-e.h +++ b/Venus_Skeleton/calibration_wall-e.h @@ -5,5 +5,10 @@ #define CAL_IR_LEFT_THRESHOLD 600 #define CAL_IR_RIGHT_THRESHOLD 600 #define CAL_SAMPLE_GRIPPER_THRESHOLD x -#define CAL_INITIAL_DISTANCE x -#define CAL_TURRET_DEGREE 7 \ No newline at end of file +#define CAL_INITIAL_DISTANCE 30.0 +#define CAL_TURRET_DEGREE 7 + +// 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 \ No newline at end of file diff --git a/Venus_Skeleton/dataTypes.h b/Venus_Skeleton/dataTypes.h index 3829da1..9021dc6 100644 --- a/Venus_Skeleton/dataTypes.h +++ b/Venus_Skeleton/dataTypes.h @@ -16,6 +16,7 @@ typedef struct { bool sample_turret_detected[NUM_TURRET_DIRECTIONS]; bool beacon_detected[NUM_TURRET_DIRECTIONS]; long obstacle_turret_distances[NUM_TURRET_DIRECTIONS]; + int compass; int robot_x[NUM_TURRET_DIRECTIONS]; int robot_y[NUM_TURRET_DIRECTIONS]; int robot_deg[NUM_TURRET_DIRECTIONS]; -- cgit v1.2.1