This program is for an autonomous robot for the competition at the Hochschule Luzern. We are one of the 32 teams. <a href="http://cruisingcrepe.wordpress.com/">http://cruisingcrepe.wordpress.com/</a> The postition control is based on this Documentation: Control of Wheeled Mobile Robots: An Experimental Overview from Alessandro De Luca, Giuseppe Oriolo, Marilena Vendittelli. For more information see here: <a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf">http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf</a>
Fork of autonomousRobotAndroid by
StateDefines/State.cpp@6:48eeb41188dd, 2013-03-23 (annotated)
- Committer:
- chrigelburri
- Date:
- Sat Mar 23 13:52:48 2013 +0000
- Revision:
- 6:48eeb41188dd
- Parent:
- 5:48a258f6335e
- Child:
- 8:696c2f9dfc62
mit link und rechten Radradius
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
chrigelburri | 0:31f7be68e52d | 1 | #include "State.h" |
chrigelburri | 0:31f7be68e52d | 2 | |
chrigelburri | 0:31f7be68e52d | 3 | using namespace std; |
chrigelburri | 0:31f7be68e52d | 4 | |
chrigelburri | 0:31f7be68e52d | 5 | // File |
chrigelburri | 0:31f7be68e52d | 6 | FILE *logp; |
chrigelburri | 0:31f7be68e52d | 7 | |
chrigelburri | 0:31f7be68e52d | 8 | // LocalFileSystem |
chrigelburri | 0:31f7be68e52d | 9 | LocalFileSystem local("local"); |
chrigelburri | 0:31f7be68e52d | 10 | |
chrigelburri | 1:6cd533a712c6 | 11 | State::State(state_t* s, RobotControl* robotControl, MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, /*HMC6352* compass,*/ AnalogIn* battery, float period) : Task(period) |
chrigelburri | 0:31f7be68e52d | 12 | { |
chrigelburri | 0:31f7be68e52d | 13 | /* get peripherals */ |
chrigelburri | 0:31f7be68e52d | 14 | this->s = s; |
chrigelburri | 0:31f7be68e52d | 15 | this->robotControl = robotControl; |
chrigelburri | 0:31f7be68e52d | 16 | this->motorControllerLeft = motorControllerLeft; |
chrigelburri | 0:31f7be68e52d | 17 | this->motorControllerRight = motorControllerRight; |
chrigelburri | 2:d8e1613dc38b | 18 | // this->compass = compass; |
chrigelburri | 0:31f7be68e52d | 19 | this->battery =battery; |
chrigelburri | 0:31f7be68e52d | 20 | this->period = period; |
chrigelburri | 0:31f7be68e52d | 21 | } |
chrigelburri | 0:31f7be68e52d | 22 | |
chrigelburri | 0:31f7be68e52d | 23 | State::~State() {} |
chrigelburri | 0:31f7be68e52d | 24 | |
chrigelburri | 0:31f7be68e52d | 25 | void State::initPlotFile(void) |
chrigelburri | 0:31f7be68e52d | 26 | { |
chrigelburri | 0:31f7be68e52d | 27 | logp = fopen("/local/plots.txt", "w"); // only write |
chrigelburri | 0:31f7be68e52d | 28 | if(logp == NULL) { |
chrigelburri | 0:31f7be68e52d | 29 | exit(1); |
chrigelburri | 0:31f7be68e52d | 30 | } else { |
chrigelburri | 0:31f7be68e52d | 31 | fprintf(logp, |
chrigelburri | 0:31f7be68e52d | 32 | "Time[ms]\t" |
chrigelburri | 0:31f7be68e52d | 33 | "BatteryVoltage[V]\t" |
chrigelburri | 0:31f7be68e52d | 34 | "NumberOfPulsesLeft\t" |
chrigelburri | 0:31f7be68e52d | 35 | "NumberOfPulsesRight\t" |
chrigelburri | 0:31f7be68e52d | 36 | "VelocityLeft[m/s]\t" |
chrigelburri | 0:31f7be68e52d | 37 | "VelocityRight[m/s]\t" |
chrigelburri | 4:3a97923ff2d4 | 38 | "VelocityCar[m/s]\t" //7 |
chrigelburri | 0:31f7be68e52d | 39 | "VelocityRotation[grad/s]\t" |
chrigelburri | 0:31f7be68e52d | 40 | "X-AxisCo-ordinate[m]\t" |
chrigelburri | 0:31f7be68e52d | 41 | "Y-AxisCo-ordinate[m]\t" |
chrigelburri | 0:31f7be68e52d | 42 | "X-AxisError[m]\t" |
chrigelburri | 0:31f7be68e52d | 43 | "X-AxisError[m]\t" |
chrigelburri | 0:31f7be68e52d | 44 | "AngleError[grad]\t" |
chrigelburri | 0:31f7be68e52d | 45 | "AngleCar[grad]\t" |
chrigelburri | 5:48a258f6335e | 46 | "SetpointX-Axis[m]\t" //15 |
chrigelburri | 0:31f7be68e52d | 47 | "SetpointY-Axis[m]\t" |
chrigelburri | 5:48a258f6335e | 48 | "SetpointAngel[grad]\t" //17 |
chrigelburri | 0:31f7be68e52d | 49 | "RightDistanceSensor[m]\t" |
chrigelburri | 0:31f7be68e52d | 50 | "CompassAngle[grad]\t" |
chrigelburri | 2:d8e1613dc38b | 51 | "CompassX-Axis\t" //20 |
chrigelburri | 0:31f7be68e52d | 52 | "CompassY-Axis\t" |
chrigelburri | 2:d8e1613dc38b | 53 | "State\t" |
chrigelburri | 2:d8e1613dc38b | 54 | "distanceToGoal[m]\t" //23 |
chrigelburri | 5:48a258f6335e | 55 | "angleToGoal[grad]\t" |
chrigelburri | 5:48a258f6335e | 56 | "thetaFromTheGoal[grad]\n"); |
chrigelburri | 0:31f7be68e52d | 57 | } |
chrigelburri | 0:31f7be68e52d | 58 | } |
chrigelburri | 0:31f7be68e52d | 59 | |
chrigelburri | 0:31f7be68e52d | 60 | |
chrigelburri | 0:31f7be68e52d | 61 | void State::savePlotFile(state_t s) |
chrigelburri | 0:31f7be68e52d | 62 | { |
chrigelburri | 0:31f7be68e52d | 63 | char buf[256]; |
chrigelburri | 0:31f7be68e52d | 64 | |
chrigelburri | 6:48eeb41188dd | 65 | sprintf(buf,"%d\t%f\t%d\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t" |
chrigelburri | 6:48eeb41188dd | 66 | "%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%d\t%f\t%f\t%f", |
chrigelburri | 0:31f7be68e52d | 67 | s.millis, |
chrigelburri | 0:31f7be68e52d | 68 | s.voltageBattery, |
chrigelburri | 0:31f7be68e52d | 69 | s.leftPulses, |
chrigelburri | 0:31f7be68e52d | 70 | s.rightPulses, |
chrigelburri | 0:31f7be68e52d | 71 | s.leftVelocity, |
chrigelburri | 0:31f7be68e52d | 72 | s.rightVelocity, |
chrigelburri | 0:31f7be68e52d | 73 | s.velocity, |
chrigelburri | 0:31f7be68e52d | 74 | s.omega, |
chrigelburri | 0:31f7be68e52d | 75 | s.xAxis, |
chrigelburri | 0:31f7be68e52d | 76 | s.yAxis, |
chrigelburri | 0:31f7be68e52d | 77 | s.xAxisError, |
chrigelburri | 0:31f7be68e52d | 78 | s.yAxisError, |
chrigelburri | 0:31f7be68e52d | 79 | s.angleError, |
chrigelburri | 0:31f7be68e52d | 80 | s.angle, |
chrigelburri | 0:31f7be68e52d | 81 | s.setxAxis, |
chrigelburri | 0:31f7be68e52d | 82 | s.setyAxis, |
chrigelburri | 0:31f7be68e52d | 83 | s.setAngle, |
chrigelburri | 0:31f7be68e52d | 84 | s.rightDist, |
chrigelburri | 0:31f7be68e52d | 85 | s.compassAngle, |
chrigelburri | 0:31f7be68e52d | 86 | s.compassxAxis, |
chrigelburri | 0:31f7be68e52d | 87 | s.compassyAxis, |
chrigelburri | 2:d8e1613dc38b | 88 | s.state, |
chrigelburri | 2:d8e1613dc38b | 89 | s.rho, |
chrigelburri | 2:d8e1613dc38b | 90 | s.lamda, |
chrigelburri | 2:d8e1613dc38b | 91 | s.delta |
chrigelburri | 0:31f7be68e52d | 92 | ); |
chrigelburri | 0:31f7be68e52d | 93 | |
chrigelburri | 0:31f7be68e52d | 94 | if (logp) |
chrigelburri | 0:31f7be68e52d | 95 | fprintf(logp, buf); |
chrigelburri | 0:31f7be68e52d | 96 | fprintf(logp, "\n"); // new line |
chrigelburri | 0:31f7be68e52d | 97 | } |
chrigelburri | 0:31f7be68e52d | 98 | |
chrigelburri | 0:31f7be68e52d | 99 | void State::savePlotText(char text[]) |
chrigelburri | 0:31f7be68e52d | 100 | { |
chrigelburri | 0:31f7be68e52d | 101 | fprintf(logp, text); |
chrigelburri | 0:31f7be68e52d | 102 | } |
chrigelburri | 0:31f7be68e52d | 103 | |
chrigelburri | 0:31f7be68e52d | 104 | void State::closePlotFile(void) |
chrigelburri | 0:31f7be68e52d | 105 | { |
chrigelburri | 0:31f7be68e52d | 106 | fclose(logp); |
chrigelburri | 0:31f7be68e52d | 107 | } |
chrigelburri | 0:31f7be68e52d | 108 | |
chrigelburri | 0:31f7be68e52d | 109 | float State::readBattery() |
chrigelburri | 0:31f7be68e52d | 110 | { |
chrigelburri | 0:31f7be68e52d | 111 | return battery->read()*BAT_MULTIPLICATOR; |
chrigelburri | 0:31f7be68e52d | 112 | } |
chrigelburri | 0:31f7be68e52d | 113 | |
chrigelburri | 0:31f7be68e52d | 114 | void State::setBatteryBit() |
chrigelburri | 0:31f7be68e52d | 115 | { |
chrigelburri | 0:31f7be68e52d | 116 | if(s->voltageBattery < BAT_MIN) { |
chrigelburri | 0:31f7be68e52d | 117 | s->state |= STATE_UNDER; |
chrigelburri | 0:31f7be68e52d | 118 | } else { |
chrigelburri | 0:31f7be68e52d | 119 | s->state &= (~STATE_UNDER); |
chrigelburri | 0:31f7be68e52d | 120 | } |
chrigelburri | 0:31f7be68e52d | 121 | } |
chrigelburri | 0:31f7be68e52d | 122 | |
chrigelburri | 0:31f7be68e52d | 123 | void State::setEnableLeftBit() |
chrigelburri | 0:31f7be68e52d | 124 | { |
chrigelburri | 0:31f7be68e52d | 125 | if(motorControllerLeft->isEnabled()) { |
chrigelburri | 0:31f7be68e52d | 126 | s->state &= (~STATE_LEFT); |
chrigelburri | 0:31f7be68e52d | 127 | } else { |
chrigelburri | 0:31f7be68e52d | 128 | s->state |= STATE_LEFT; |
chrigelburri | 0:31f7be68e52d | 129 | } |
chrigelburri | 0:31f7be68e52d | 130 | } |
chrigelburri | 0:31f7be68e52d | 131 | |
chrigelburri | 0:31f7be68e52d | 132 | void State::setEnableRightBit() |
chrigelburri | 0:31f7be68e52d | 133 | { |
chrigelburri | 0:31f7be68e52d | 134 | if(motorControllerRight->isEnabled()) { |
chrigelburri | 0:31f7be68e52d | 135 | s->state &= (~STATE_RIGHT); |
chrigelburri | 0:31f7be68e52d | 136 | } else { |
chrigelburri | 0:31f7be68e52d | 137 | s->state |= STATE_RIGHT; |
chrigelburri | 0:31f7be68e52d | 138 | } |
chrigelburri | 0:31f7be68e52d | 139 | } |
chrigelburri | 0:31f7be68e52d | 140 | |
chrigelburri | 0:31f7be68e52d | 141 | void State::startTimerFromZero() |
chrigelburri | 0:31f7be68e52d | 142 | { |
chrigelburri | 0:31f7be68e52d | 143 | timer.reset(); |
chrigelburri | 0:31f7be68e52d | 144 | timer.start(); |
chrigelburri | 0:31f7be68e52d | 145 | } |
chrigelburri | 0:31f7be68e52d | 146 | |
chrigelburri | 0:31f7be68e52d | 147 | void State::run() |
chrigelburri | 0:31f7be68e52d | 148 | { |
chrigelburri | 0:31f7be68e52d | 149 | s->millis = timer.read_ms(); |
chrigelburri | 0:31f7be68e52d | 150 | s->voltageBattery = readBattery(); |
chrigelburri | 0:31f7be68e52d | 151 | s->leftPulses = - motorControllerLeft->getPulses(); |
chrigelburri | 0:31f7be68e52d | 152 | s->rightPulses = motorControllerRight->getPulses(); |
chrigelburri | 6:48eeb41188dd | 153 | s->leftVelocity = motorControllerLeft->getActualSpeed() * |
chrigelburri | 6:48eeb41188dd | 154 | 2.0f * WHEEL_RADIUS_LEFT * PI; |
chrigelburri | 6:48eeb41188dd | 155 | s->rightVelocity = - motorControllerRight->getActualSpeed()* |
chrigelburri | 6:48eeb41188dd | 156 | 2.0f * WHEEL_RADIUS_RIGHT * PI; |
chrigelburri | 0:31f7be68e52d | 157 | s->velocity = robotControl->getActualSpeed(); |
chrigelburri | 0:31f7be68e52d | 158 | s->omega = robotControl->getActualOmega(); |
chrigelburri | 0:31f7be68e52d | 159 | s->xAxis = robotControl->getxActualPosition(); |
chrigelburri | 0:31f7be68e52d | 160 | s->yAxis = robotControl->getyActualPosition(); |
chrigelburri | 0:31f7be68e52d | 161 | s->angle = robotControl->getActualTheta() * 180 / PI; |
chrigelburri | 0:31f7be68e52d | 162 | s->xAxisError = robotControl->getxPositionError(); |
chrigelburri | 0:31f7be68e52d | 163 | s->yAxisError = robotControl->getyPositionError(); |
chrigelburri | 0:31f7be68e52d | 164 | s->angleError = robotControl->getThetaError() * 180 / PI; |
chrigelburri | 5:48a258f6335e | 165 | s->setxAxis = robotControl->getDesiredxPosition(); |
chrigelburri | 5:48a258f6335e | 166 | s->setyAxis = robotControl->getDesiredyPosition(); |
chrigelburri | 5:48a258f6335e | 167 | s->setAngle = robotControl->getDesiredTheta() * 180 / PI; |
chrigelburri | 2:d8e1613dc38b | 168 | // s->compassAngle = compass->getFilteredAngle() * 180 / PI; |
chrigelburri | 0:31f7be68e52d | 169 | |
chrigelburri | 2:d8e1613dc38b | 170 | s->rho = robotControl->getDistanceError(); |
chrigelburri | 2:d8e1613dc38b | 171 | s->lamda = robotControl->getThetaErrorToGoal() * 180 / PI; |
chrigelburri | 2:d8e1613dc38b | 172 | s->delta = robotControl->getThetaGoal() * 180 / PI; |
chrigelburri | 0:31f7be68e52d | 173 | |
chrigelburri | 0:31f7be68e52d | 174 | setBatteryBit(); |
chrigelburri | 0:31f7be68e52d | 175 | setEnableLeftBit(); |
chrigelburri | 0:31f7be68e52d | 176 | setEnableRightBit(); |
chrigelburri | 0:31f7be68e52d | 177 | } |