Revision 2330e415
| devices/DiWheelDrive/global.hpp | ||
|---|---|---|
| 164 | 164 |
DiWheelDrive robot; |
| 165 | 165 |
|
| 166 | 166 |
UserThread userThread; |
| 167 |
int rpmForward[2] = {25,25};
|
|
| 168 |
int rpmSoftLeft[2] = {15,25};
|
|
| 169 |
int rpmHardLeft[2] = {10,25};
|
|
| 167 |
int rpmForward[2] = {20,20};
|
|
| 168 |
int rpmSoftLeft[2] = {10,20};
|
|
| 169 |
int rpmHardLeft[2] = {5,20};
|
|
| 170 | 170 |
int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]};
|
| 171 | 171 |
int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};
|
| 172 | 172 |
|
| devices/DiWheelDrive/linefollow2.cpp | ||
|---|---|---|
| 1 | 1 |
#include "global.hpp" |
| 2 | 2 |
#include "linefollow2.hpp" |
| 3 |
#include <cmath> |
|
| 3 | 4 |
|
| 4 | 5 |
|
| 5 | 6 |
void LineFollow::printSensorData(){
|
| 6 | 7 |
chprintf((BaseSequentialStream*) &SD1, "Test!"); |
| 7 | 8 |
} |
| 8 | 9 |
|
| 9 |
void LineFollow::followLine(int vcnl4020Proximity[4], Global *global){
|
|
| 10 |
chprintf((BaseSequentialStream*) &SD1, "Proximity: WL:0x%04X FL:0x%04X FR:0x%04X WR:0x%04X\n", |
|
| 11 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], |
|
| 12 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
|
| 13 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], |
|
| 14 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]); |
|
| 15 |
global->motorcontrol.printGains(); |
|
| 10 |
void LineFollow::followLine(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){
|
|
| 11 |
|
|
| 12 |
chprintf((BaseSequentialStream*) &SD1, "SP: %d,\n", SetPoint); |
|
| 13 |
|
|
| 14 |
// chprintf((BaseSequentialStream*) &SD1, "Proximity: WL:0x%04X FL:0x%04X FR:0x%04X WR:0x%04X\n", |
|
| 15 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], |
|
| 16 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
|
| 17 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], |
|
| 18 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]); |
|
| 19 |
// global->motorcontrol.printGains(); |
|
| 20 |
// chprintf((BaseSequentialStream*) &SD1, "Speed -- Left: %d, Right: %d\n", global->motorcontrol.getCurrentRPMLeft(), global->motorcontrol.getCurrentRPMRight()); |
|
| 21 |
|
|
| 22 |
|
|
| 23 |
// float speedL = global->motorcontrol.getCurrentRPMLeft(); |
|
| 24 |
// float speedR = global->motorcontrol.getCurrentRPMRight(); |
|
| 25 |
// chprintf((BaseSequentialStream*) &SD1, "After motor request SP: %f,\n", SetPoint); |
|
| 26 |
// Process value |
|
| 27 |
float processV = static_cast< float >((vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] + vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT])); |
|
| 28 |
// chprintf((BaseSequentialStream*) &SD1, "PV: %f,\n", processV); |
|
| 29 |
// chprintf((BaseSequentialStream*) &SD1, "After PV SP: %f,\n", SetPoint); |
|
| 30 |
float error = SetPoint - processV; |
|
| 31 |
float d_term = old_error - error; |
|
| 32 |
// chprintf((BaseSequentialStream*) &SD1, "After Error SP: %f,\n", SetPoint); |
|
| 33 |
// chprintf((BaseSequentialStream*) &SD1, "Error: %f,\n", error); |
|
| 34 |
acc_sum = 0.5 * acc_sum + error; |
|
| 35 |
int new_speed = static_cast< int >(Kp * error + Ki*acc_sum + Kd*d_term); |
|
| 36 |
old_error = error; |
|
| 37 |
chprintf((BaseSequentialStream*) &SD1, "Error: %f,\n", error); |
|
| 38 |
chprintf((BaseSequentialStream*) &SD1, "Dterm: %f,\n", d_term); |
|
| 39 |
chprintf((BaseSequentialStream*) &SD1, "Iterm: %f,\n", acc_sum); |
|
| 40 |
chprintf((BaseSequentialStream*) &SD1, "New Speed: %d,\n", new_speed); |
|
| 41 |
// chprintf((BaseSequentialStream*) &SD1, "New Speed: %f, Sum: %f, SP: %f, processV: %f, K_p: %f, K_i: %f \n", new_speed, acc_sum, SetPoint, processV, Kp, Ki); |
|
| 42 |
|
|
| 43 |
// int forward = 15; |
|
| 44 |
// int l_speed = forward - new_speed; |
|
| 45 |
// int r_speed = forward + new_speed; |
|
| 46 |
|
|
| 47 |
// if (l_speed ) |
|
| 48 |
|
|
| 49 |
rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = 15 + new_speed; |
|
| 50 |
rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = 15 - new_speed; |
|
| 51 |
|
|
| 52 |
chprintf((BaseSequentialStream*) &SD1, "Speed L: %d, R: %d\n", rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL], rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL]); |
|
| 53 |
|
|
| 16 | 54 |
} |
| devices/DiWheelDrive/linefollow2.hpp | ||
|---|---|---|
| 11 | 11 |
{
|
| 12 | 12 |
public: |
| 13 | 13 |
void printSensorData(); |
| 14 |
void followLine(int vcnl4020Proximity[4], Global *global); |
|
| 14 |
void followLine(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global); |
|
| 15 |
float SetPoint = 0x4000; // (0x1800+0x2800) >> 8 |
|
| 16 |
float Kp = 0.001; |
|
| 17 |
float Ki = 0.00001; |
|
| 18 |
float Kd = 0.1; |
|
| 19 |
float acc_sum = 0; |
|
| 20 |
float old_error = 0; |
|
| 21 |
|
|
| 15 | 22 |
}; |
| 16 | 23 |
|
| 17 | 24 |
} // end of namespace amiro |
| devices/DiWheelDrive/userthread.cpp | ||
|---|---|---|
| 108 | 108 |
void copyRpmSpeed(const int (&source)[2], int (&target)[2]) {
|
| 109 | 109 |
target[constants::DiWheelDrive::LEFT_WHEEL] = source[constants::DiWheelDrive::LEFT_WHEEL]; |
| 110 | 110 |
target[constants::DiWheelDrive::RIGHT_WHEEL] = source[constants::DiWheelDrive::RIGHT_WHEEL]; |
| 111 |
chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]); |
|
| 111 |
// chprintf((BaseSequentialStream*) &SD1, "Speed left: %d, Speed right: %d\r\n", target[0], target[1]);
|
|
| 112 | 112 |
} |
| 113 | 113 |
|
| 114 | 114 |
// Fuzzyfication of the sensor values |
| ... | ... | |
| 311 | 311 |
// Hard deviatio to right |
| 312 | 312 |
}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY |
| 313 | 313 |
&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE){
|
| 314 |
copyRpmSpeed(global.rpmHardLeft, rpmFuzzyCtrl);
|
|
| 314 |
copyRpmSpeed(rpmTurnLeft, rpmFuzzyCtrl);
|
|
| 315 | 315 |
// Hard deviation to left |
| 316 | 316 |
}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == GREY |
| 317 | 317 |
&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == BLACK){
|
| 318 |
copyRpmSpeed(global.rpmHardRight, rpmFuzzyCtrl);
|
|
| 319 |
// Turn if white
|
|
| 318 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
|
|
| 319 |
// stop if white
|
|
| 320 | 320 |
}else if (member[constants::DiWheelDrive::PROX_FRONT_LEFT] == WHITE |
| 321 | 321 |
&& member[constants::DiWheelDrive::PROX_FRONT_RIGHT] == WHITE ){
|
| 322 |
copyRpmSpeed(rpmTurnRight, rpmFuzzyCtrl);
|
|
| 322 |
copyRpmSpeed(rpmHalt, rpmFuzzyCtrl);
|
|
| 323 | 323 |
} |
| 324 | 324 |
} |
| 325 | 325 |
|
| ... | ... | |
| 452 | 452 |
vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
| 453 | 453 |
vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
| 454 | 454 |
} |
| 455 |
lf.followLine(vcnl4020Proximity, &global); |
|
| 455 |
lf.followLine(vcnl4020Proximity, rpmFuzzyCtrl, &global);
|
|
| 456 | 456 |
// chprintf((BaseSequentialStream*) &SD1, "0x%04X 0x%04X 0x%04X 0x%04X\n", |
| 457 | 457 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], |
| 458 | 458 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
| ... | ... | |
| 461 | 461 |
//if (configLineFollowing==2) |
| 462 | 462 |
// lineFollownew |
| 463 | 463 |
//else |
| 464 |
lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl);
|
|
| 465 |
setRpmSpeed(rpmFuzzyCtrl); |
|
| 464 |
// lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl, &global);
|
|
| 465 |
// setRpmSpeed(rpmFuzzyCtrl);
|
|
| 466 | 466 |
} |
| 467 | 467 |
|
| 468 |
// this->sleep(US2ST(5)); |
|
| 468 | 469 |
this->sleep(CAN::UPDATE_PERIOD); |
| 469 | 470 |
} |
| 470 | 471 |
|
| 471 | 472 |
return RDY_OK; |
| 472 | 473 |
} |
| 473 |
|
|
Also available in: Unified diff