From a1c2581267a6f6c81dbbdc257085d0e4de029604 Mon Sep 17 00:00:00 2001
From: galberding <galberding@techfak.uni-bielefeld.de>
Date: Mon, 31 Aug 2020 14:50:18 +0200
Subject: [PATCH] Use sensorvalues for turning
---
devices/DiWheelDrive/DiWheelDrive.cpp | 2 +-
devices/DiWheelDrive/global.hpp | 8 +-
devices/DiWheelDrive/linefollow.cpp | 31 +++---
devices/DiWheelDrive/linefollow.hpp | 22 +++--
devices/DiWheelDrive/main.cpp | 54 ++++++++++-
devices/DiWheelDrive/userthread.cpp | 135 ++++++++++++++++++--------
devices/DiWheelDrive/userthread.hpp | 2 +-
include/amiro/Constants.h | 1 +
8 files changed, 184 insertions(+), 71 deletions(-)
diff --git a/devices/DiWheelDrive/DiWheelDrive.cpp b/devices/DiWheelDrive/DiWheelDrive.cpp
index f5744c6b..3f257a80 100644
--- a/devices/DiWheelDrive/DiWheelDrive.cpp
+++ b/devices/DiWheelDrive/DiWheelDrive.cpp
@@ -115,7 +115,7 @@ msg_t DiWheelDrive::receiveMessage(CANRxFrame *frame) {
case CAN::SET_LINE_FOLLOW_MSG:
// chprintf((BaseSequentialStream*) &SD1, "Received Strategy!\n");
if (frame->DLC == 1) {
- global.lfStrategy = frame->data8[0];
+ global.msgContent = frame->data8[0];
global.msgReceived = true;
// return RDY_OK;
}
diff --git a/devices/DiWheelDrive/global.hpp b/devices/DiWheelDrive/global.hpp
index a7919b60..c0b07d9a 100644
--- a/devices/DiWheelDrive/global.hpp
+++ b/devices/DiWheelDrive/global.hpp
@@ -165,7 +165,7 @@ public:
DiWheelDrive robot;
UserThread userThread;
- int rpmForward[2] = {30,30};
+ int rpmForward[2] = {20,20};
int rpmSoftLeft[2] = {10,20};
int rpmHardLeft[2] = {5,20};
int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]};
@@ -212,7 +212,7 @@ public:
// 1 EDGE_LEFT
// 2 FUZZY
// 3 DOCKING
- int lfStrategy = 3;
+ int msgContent = 3;
// CAN communication line:
// Will be set to true when message was received
@@ -223,7 +223,9 @@ public:
// uint8_t actualCharge = 0;
// Debugging stuff --------------
- int forwardSpeed = 5;
+
+ //TODO: Structure the variables
+ int forwardSpeed = 5; // declares the forwardspeed
int enableRecord = 0;
bool triggerCan = false;
diff --git a/devices/DiWheelDrive/linefollow.cpp b/devices/DiWheelDrive/linefollow.cpp
index 2d2a4ffb..d9c58de5 100644
--- a/devices/DiWheelDrive/linefollow.cpp
+++ b/devices/DiWheelDrive/linefollow.cpp
@@ -57,11 +57,11 @@ int LineFollow::getError(){
// Get actual sensor data of both front sensors
int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
- // int targetL = global->linePID.threshProxyL;
- // int targetR = global->linePID.threshProxyR;
+
int targetL = global->linePID.BThresh;
int targetR = global->linePID.WThresh;
int error = 0;
+ // Calculate the error according to the current strategy
switch (this->strategy)
{
case LineFollowStrategy::EDGE_RIGHT:
@@ -82,14 +82,7 @@ int LineFollow::getError(){
default:
break;
}
- // Debugging stuff ------
- // if (global->enableRecord){
- // global->senseRec[global->sensSamples].error = error;
- // global->senseRec[global->sensSamples].FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
- // global->senseRec[global->sensSamples].FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
- // global->sensSamples++;
- // }
- // ----------------------
+
// Register white values
if (FL+FR > global->threshWhite){
whiteFlag = 1;
@@ -122,10 +115,11 @@ int LineFollow::followLine(int (&rpmSpeed)[2]){
break;
case LineFollowStrategy::REVERSE:
+ // Perform straight
correctionSpeed = -getPidCorrectionSpeed();
- rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -1000000*global->forwardSpeed;
+ rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -SPEED_M_TO_U*global->forwardSpeed;
- rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -1000000*global->forwardSpeed;
+ rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -SPEED_M_TO_U*global->forwardSpeed;
break;
@@ -133,9 +127,9 @@ int LineFollow::followLine(int (&rpmSpeed)[2]){
correctionSpeed = getPidCorrectionSpeed();
// chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed, global->threshWhite);
- rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = 1000000*global->forwardSpeed + correctionSpeed;
+ rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = SPEED_M_TO_U*global->forwardSpeed + correctionSpeed;
- rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = 1000000*global->forwardSpeed - correctionSpeed;
+ rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = SPEED_M_TO_U*global->forwardSpeed - correctionSpeed;
break;
}
@@ -156,8 +150,10 @@ int LineFollow::followLine(int (&rpmSpeed)[2]){
int LineFollow::getPidCorrectionSpeed(){
int32_t error = getError();
int32_t sloap = oldError - error ;
- // int correctionSpeed = (int) (global->K_p*error + Ki*accumHist - global->K_d*sloap);
+
+ // Calculate the correction speed for the current step
int32_t correctionSpeed = (int32_t) (K_p*error + K_i*accumHist + K_d*sloap);
+ //Update the old error value
oldError = error;
// Overflow Protection
@@ -171,9 +167,14 @@ int LineFollow::getPidCorrectionSpeed(){
}else {
accumHist += error;
}
+
+ // ----------------------------------------------------------------------------------------------------
+ // Debugging: Track the biggest occurring error value
+
if (abs(error) > global->maxDist.error){
global->maxDist.error = error;
}
+ // ----------------------------------------------------------------------------------------------------
return correctionSpeed;
}
diff --git a/devices/DiWheelDrive/linefollow.hpp b/devices/DiWheelDrive/linefollow.hpp
index 01aa8c90..98a850f9 100644
--- a/devices/DiWheelDrive/linefollow.hpp
+++ b/devices/DiWheelDrive/linefollow.hpp
@@ -6,10 +6,12 @@
#include <amiroosconf.h>
#define RAND_TRESH 16000
-#define MAX_CORRECTED_SPEED 1000000*100
+// Multiplication factor to convert m/s to um/s
+#define SPEED_M_TO_U 1000000
+#define MAX_CORRECTED_SPEED SPEED_M_TO_U*100
namespace amiro {
-
+
enum LineFollowStrategy{
EDGE_LEFT, // driving on the left edge of a black line
TRANSITION_L_R, // Transition from left to right edge
@@ -37,19 +39,19 @@ public:
LineFollow(Global *global, LineFollowStrategy strategy);
/**
* Entry method which should be called to follow a line.
- *
+ *
* @param rpmSpeed speed that will be determained
- * @return white flag, 1 if white is detected
+ * @return white flag, 1 if white is detected
*/
int followLine(int (&rpmSpeed)[2]);
/**
* Setter for LineFollowStrategy which triggers transition behavior.
- *
+ *
* To prevent random turns while changeing the strategy a transition state is set.
* In case the strategy needs to be switched immediately use promptStrategyChange().
- *
- * @param strategy
+ *
+ * @param strategy
*/
void setStrategy(LineFollowStrategy strategy);
void promptStrategyChange(LineFollowStrategy strategy);
@@ -87,17 +89,17 @@ private:
/**
* Error calculation while changing from one EDGE_* strategy to another.
* This prevents the AMiRo from random turns while switching strategies.
- *
+ *
* @param FL value of left front sensor
* @param FR value of right front sensor
- * @param targetL left threshold
+ * @param targetL left threshold
* @param targetR right threshold
*/
int transitionError(int FL, int FR, int targetL, int targetR);
/**
* Use the error according to the strategy to calculate the correction speed.
- * Currently only the P part of the PID controller is used to calculate the
+ * Currently only the P part of the PID controller is used to calculate the
* correction speed.
*/
int getPidCorrectionSpeed();
diff --git a/devices/DiWheelDrive/main.cpp b/devices/DiWheelDrive/main.cpp
index 408a01b0..0889caa7 100644
--- a/devices/DiWheelDrive/main.cpp
+++ b/devices/DiWheelDrive/main.cpp
@@ -910,6 +910,7 @@ void shellRequestPrintCoordinate(BaseSequentialStream *chp, int argc, char *argv
}
+
void shellRequestErrorInfo(BaseSequentialStream *chp, int argc, char *argv[]){
// Print out the error info collected. Clear buffer after calling
chprintf(chp, "Error Info\n");
@@ -988,10 +989,57 @@ void shellRequestMapTest(BaseSequentialStream *chp, int argc, char *argv[]) {
chprintf(chp, " +------> 1 | |\n");
chprintf(chp, " +---+-------------------+\n");
// BaseThread::sleep(MS2ST(250));
- global.lfStrategy = msg_content::MSG_TEST_MAP_STATE;
+ global.msgContent = msg_content::MSG_TEST_MAP_STATE;
+ global.msgReceived = true;
+
+ }
+
+// Calibrate the Black bottom sensor value
+void shellRequestCalibBlack(BaseSequentialStream *chp, int argc, char *argv[]) {
+ global.msgContent = msg_content::MSG_CALIBRATE_BLACK;
+ global.msgReceived = true;
+ }
+
+// Calibrate white value
+void shellRequestCalibWhite(BaseSequentialStream *chp, int argc, char *argv[]) {
+ global.msgContent = msg_content::MSG_CALIBRATE_WHITE;
+ global.msgReceived = true;
+ }
+
+
+// Calibrate white value
+void shellRequestDock(BaseSequentialStream *chp, int argc, char *argv[]) {
+ // Set Amiro in docking mode
+ global.msgContent = msg_content::MSG_DOCK;
global.msgReceived = true;
+ }
+
+
+void sellRequestSpeedStage(BaseSequentialStream *chp, int argc, char *argv[]) {
+ int level;
+ if (argc == 1){
+ chprintf(chp, "Speed Level %i \n", atoi(argv[0]));
+ level = atoi(argv[0]);
+
+ } else {
+ chprintf(chp, "Usage: setSpeedLevel <1|2|3> \n");
}
+ switch(level){
+ case 1:
+ global.forwardSpeed = -10;
+ break;
+ case 2:
+ global.forwardSpeed = 15;
+ break;
+ case 3:
+ global.forwardSpeed = 20;
+ break;
+ default:
+ chprintf(chp, "Usage: setSpeedLevel <1|2|3> \n");
+ break;
+ }
+}
static const ShellCommand commands[] = {
{"shutdown", shellRequestShutdown},
@@ -1028,6 +1076,10 @@ static const ShellCommand commands[] = {
{"checkPowerPins", shellRequestCheckPower},
{"stateInfos", shellRequestErrorInfo},
{"test", shellRequestMapTest},
+ {"bottomCalibrateBlack", shellRequestCalibBlack},
+ {"bottomCalibrateWhite", shellRequestCalibWhite},
+ {"setSpeedLevel", sellRequestSpeedStage},
+ {"dock", shellRequestDock },
{NULL, NULL}};
static const ShellConfig shell_cfg1 = {
diff --git a/devices/DiWheelDrive/userthread.cpp b/devices/DiWheelDrive/userthread.cpp
index 68e8737c..28e830bf 100644
--- a/devices/DiWheelDrive/userthread.cpp
+++ b/devices/DiWheelDrive/userthread.cpp
@@ -209,7 +209,7 @@ int UserThread::checkDockingSuccess(){
// Amiro moved, docking was not successful
// if ((start.x + stop_.x) || (start.y + stop_.y)){
- if (abs(start.x - stop_.x) > 200 /* || (start.y + stop_.y) */){
+ if (abs(start.x - stop_.x) > 300 /* || (start.y + stop_.y) */){
lightAllLeds(Color::RED);
// Enable Motor again if docking was not successful
global.motorcontrol.setMotorEnable(true);
@@ -220,7 +220,7 @@ int UserThread::checkDockingSuccess(){
}
// this->sleep(500);
- lightAllLeds(Color::BLACK);
+ // lightAllLeds(Color::BLACK);
return success;
}
@@ -309,7 +309,7 @@ UserThread::main()
} else if(global.msgReceived){
global.msgReceived = false;
// running = true;
- switch(global.lfStrategy){
+ switch(global.msgContent){
case msg_content::MSG_START:
newState = ut_states::UT_CALIBRATION_CHECK;
break;
@@ -372,6 +372,9 @@ UserThread::main()
// Get sensor data
uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
+ uint16_t FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
+ uint16_t FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
+
for(int i=0; i<8;i++){
rProx[i] = global.robot.getProximityRingValue(i);
}
@@ -399,11 +402,15 @@ UserThread::main()
This values will be used by the line follow object
*/
+
proxCalib.buf = 0;
+ // Only calibrate the sensors if robot is on black surface
if(proxCalib.calibrateBlack){
chprintf((BaseSequentialStream*)&global.sercanmux1, "Black Calibration, Place AMiRo on black Surface!\n");
global.robot.calibrate();
}
+
+ // Get mean sensor value for pid regulation
for(int i=0; i <= proxCalib.meanWindow; i++){
proxCalib.buf += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset()
+ global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
@@ -411,6 +418,7 @@ UserThread::main()
}
proxCalib.buf = proxCalib.buf / (2*proxCalib.meanWindow);
+ // Store mean value
if(proxCalib.calibrateBlack){
global.linePID.BThresh = proxCalib.buf;
}else {
@@ -516,38 +524,71 @@ UserThread::main()
}
// ---------------------------------------
case ut_states::UT_DETECT_STATION:
+ // Set Amiro in detection state. It is expected that a docking station is
+ // positioned on the right edge.
- if (global.forwardSpeed != DETECTION_SPEED){
- global.forwardSpeed = DETECTION_SPEED;
- }
- if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
+ // Lower the speed for more accuracy when reaching the station
+ if (global.forwardSpeed != DETECTION_SPEED){
+ global.forwardSpeed = DETECTION_SPEED;
+ }
+
+ // Switch to the right edge
+ if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
lf.setStrategy(LineFollowStrategy::EDGE_RIGHT);
}
- lf.followLine(rpmSpeed);
- setRpmSpeed(rpmSpeed);
- // // Detect marker before docking station
- // if ((WL+WR) < PROXY_WHEEL_THRESH){
- // Use proxy ring
- if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){
-
- setRpmSpeed(stop);
- checkForMotion();
- // 180° Rotation
- global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
- // BaseThread::sleep(8000);
- checkForMotion();
- newState = ut_states::UT_CORRECT_POSITIONING;
+ // Continue line following until station is not reached
+ lf.followLine(rpmSpeed);
+ setRpmSpeed(rpmSpeed);
+ // // Detect marker before docking station
+ // if ((WL+WR) < PROXY_WHEEL_THRESH){
+ // Use proxy ring
+
+ // Front sensors detect the station
+ // If thresh reached start with the docking procedure
+ if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){
+
+ setRpmSpeed(stop);
+ checkForMotion();
+ // 180° Rotation
+ // global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
+ // BaseThread::sleep(8000);
+ // checkForMotion();
+ newState = ut_states::UT_STATION_POSITIONING_TURN;
}
break;
// ---------------------------------------
+
+ case ut_states::UT_STATION_POSITIONING_TURN:{
+ // TODO: Manually rotate 180°
+
+ rpmSpeed[0] = SPEED_CONVERSION_FACTOR * CHARGING_SPEED;
+ rpmSpeed[1] = -SPEED_CONVERSION_FACTOR * CHARGING_SPEED;
+ setRpmSpeed(rpmSpeed);
+
+ if (FR <= (FL / 2)){
+ setRpmSpeed(stop);
+ checkForMotion();
+ newState = ut_states::UT_CORRECT_POSITIONING;
+ }
+
+ break;
+ }
+ // ---------------------------------------
case ut_states::UT_CORRECT_POSITIONING:
+
+ // Ensure slow speed
+ // TODO: May be obsolete
if (global.forwardSpeed != CHARGING_SPEED){
global.forwardSpeed = CHARGING_SPEED;
}
+ // Ensure that the strategy is set to Left edge
+ // This is due to the 180° turn in the previous state
if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
}
+
+ // Follow line position the amiro straight in front of the charging station
lf.followLine(rpmSpeed);
setRpmSpeed(rpmSpeed);
@@ -566,7 +607,7 @@ UserThread::main()
}
lf.followLine(rpmSpeed);
setRpmSpeed(rpmSpeed);
- // utCount.stateTime++;
+ utCount.stateTime++;
// Docking is only successful if Deviation is in range and sensors are at their max values.
if((rProx[0] >= PROX_MAX_VAL)
@@ -696,12 +737,12 @@ UserThread::main()
global.motorcontrol.setMotorEnable(true);
global.robot.requestCharging(0);
// TODO: Soft release when docking falsely
- if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
- newState = ut_states::UT_RELEASE_TO_CORRECT;
- } else {
- newState = ut_states::UT_RELEASE_TO_CORRECT; //ut_states::UT_CORRECT_POSITIONING;
- }
-
+ // if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
+ // newState = ut_states::UT_RELEASE_TO_CORRECT;
+ // } else {
+ // newState = ut_states::UT_RELEASE_TO_CORRECT; //ut_states::UT_CORRECT_POSITIONING;
+ // }
+ newState = ut_states::UT_RELEASE_TO_CORRECT;
if (utCount.errorCount > DOCKING_ERROR_THRESH){
newState = ut_states::UT_DOCKING_ERROR;
}
@@ -711,18 +752,32 @@ UserThread::main()
// ---------------------------------------
case ut_states::UT_RELEASE_TO_CORRECT:
- global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
- checkForMotion();
- // move 1cm forward
- global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
- checkForMotion();
- // rotate back
- global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION);
- checkForMotion();
-
- global.distcontrol.setTargetPosition(1500, 0, ROTATION_DURATION);
- checkForMotion();
- newState = ut_states::UT_CORRECT_POSITIONING;
+ // global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
+ // checkForMotion();
+ // // move 1cm forward
+ // global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
+ // checkForMotion();
+ // // rotate back
+ // global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION);
+ // checkForMotion();
+
+ // global.distcontrol.setTargetPosition(1500, 0, ROTATION_DURATION);
+
+
+ utCount.stateCount++;
+ if(utCount.stateCount < 150){
+ rpmSpeed[0] = -SPEED_CONVERSION_FACTOR * CHARGING_SPEED;
+ rpmSpeed[1] = SPEED_CONVERSION_FACTOR * CHARGING_SPEED;
+ setRpmSpeed(rpmSpeed);
+ }else if(utCount.stateCount < 180){
+ rpmSpeed[0] = SPEED_CONVERSION_FACTOR * CHARGING_SPEED;
+ rpmSpeed[1] = SPEED_CONVERSION_FACTOR * CHARGING_SPEED;
+ setRpmSpeed(rpmSpeed);
+ }else{
+ newState = ut_states::UT_STATION_POSITIONING_TURN ;
+ utCount.stateCount = 0;
+ }
+ // checkForMotion();
break;
// ---------------------------------------
case ut_states::UT_CHARGING:
diff --git a/devices/DiWheelDrive/userthread.hpp b/devices/DiWheelDrive/userthread.hpp
index 392ce843..a15e9f7e 100644
--- a/devices/DiWheelDrive/userthread.hpp
+++ b/devices/DiWheelDrive/userthread.hpp
@@ -52,7 +52,7 @@
#define CAN_TRANSMIT_STATE_THRESH 50
#define PROX_DEVIATION_MEAN_WINDOW 3
#define MAX_DEVIATION_CORRECTIONS 4
-#define MAX_DEVIATION_FACTOR 35
+#define MAX_DEVIATION_FACTOR 45
#define DEVIATION_CORRECTION_DURATION 900
#define DEVIATION_CORRECTION_SPEED 2000000
#define DEVIATION_CORRECTION_VALUE (DEVIATION_CORRECTION_SPEED / 2)
diff --git a/include/amiro/Constants.h b/include/amiro/Constants.h
index 6efd6536..84e4c477 100644
--- a/include/amiro/Constants.h
+++ b/include/amiro/Constants.h
@@ -86,6 +86,7 @@ enum ut_states : int8_t {
UT_DEVIATION_CORRECTION = 16,
UT_TEST_MAP_STATE = 17,
UT_TEST_MAP_AUTO_STATE = 18,
+ UT_STATION_POSITIONING_TURN = 19,
UT_DOCKING_ERROR = -1,
UT_REVERSE_TIMEOUT_ERROR = -2,
UT_CALIBRATION_ERROR = -3,
--
GitLab