This program is for an autonomous robot for the competition at the Hochschule Luzern. http://cruisingcrepe.wordpress.com/ We are one of the 32 teams. http://cruisingcrepe.wordpress.com/ 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: http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf
Fork of autonomous Robot Android by
StateDefines/State.cpp@4:3a97923ff2d4, 2013-03-08 (annotated)
- Committer:
- chrigelburri
- Date:
- Fri Mar 08 09:54:34 2013 +0000
- Revision:
- 4:3a97923ff2d4
- Parent:
- 2:d8e1613dc38b
- Child:
- 5:48a258f6335e
f?hrt bis zum qr code und dannach vor garage 4;
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 | 0:31f7be68e52d | 46 | "SetpointX-Axis[m]\t" |
chrigelburri | 0:31f7be68e52d | 47 | "SetpointY-Axis[m]\t" |
chrigelburri | 0:31f7be68e52d | 48 | "SetpointAngel[grad]\t" |
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 | 2:d8e1613dc38b | 55 | "angleToGoal[rad]\t" |
chrigelburri | 2:d8e1613dc38b | 56 | "thetaFromTheGoal[rad]\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 | 2:d8e1613dc38b | 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%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 | 66 | s.millis, |
chrigelburri | 0:31f7be68e52d | 67 | s.voltageBattery, |
chrigelburri | 0:31f7be68e52d | 68 | s.leftPulses, |
chrigelburri | 0:31f7be68e52d | 69 | s.rightPulses, |
chrigelburri | 0:31f7be68e52d | 70 | s.leftVelocity, |
chrigelburri | 0:31f7be68e52d | 71 | s.rightVelocity, |
chrigelburri | 0:31f7be68e52d | 72 | s.velocity, |
chrigelburri | 0:31f7be68e52d | 73 | s.omega, |
chrigelburri | 0:31f7be68e52d | 74 | s.xAxis, |
chrigelburri | 0:31f7be68e52d | 75 | s.yAxis, |
chrigelburri | 0:31f7be68e52d | 76 | s.xAxisError, |
chrigelburri | 0:31f7be68e52d | 77 | s.yAxisError, |
chrigelburri | 0:31f7be68e52d | 78 | s.angleError, |
chrigelburri | 0:31f7be68e52d | 79 | s.angle, |
chrigelburri | 0:31f7be68e52d | 80 | s.setxAxis, |
chrigelburri | 0:31f7be68e52d | 81 | s.setyAxis, |
chrigelburri | 0:31f7be68e52d | 82 | s.setAngle, |
chrigelburri | 0:31f7be68e52d | 83 | s.rightDist, |
chrigelburri | 0:31f7be68e52d | 84 | s.compassAngle, |
chrigelburri | 0:31f7be68e52d | 85 | s.compassxAxis, |
chrigelburri | 0:31f7be68e52d | 86 | s.compassyAxis, |
chrigelburri | 2:d8e1613dc38b | 87 | s.state, |
chrigelburri | 2:d8e1613dc38b | 88 | s.rho, |
chrigelburri | 2:d8e1613dc38b | 89 | s.lamda, |
chrigelburri | 2:d8e1613dc38b | 90 | s.delta |
chrigelburri | 0:31f7be68e52d | 91 | ); |
chrigelburri | 0:31f7be68e52d | 92 | |
chrigelburri | 0:31f7be68e52d | 93 | if (logp) |
chrigelburri | 0:31f7be68e52d | 94 | fprintf(logp, buf); |
chrigelburri | 0:31f7be68e52d | 95 | fprintf(logp, "\n"); // new line |
chrigelburri | 0:31f7be68e52d | 96 | } |
chrigelburri | 0:31f7be68e52d | 97 | |
chrigelburri | 0:31f7be68e52d | 98 | void State::savePlotText(char text[]) |
chrigelburri | 0:31f7be68e52d | 99 | { |
chrigelburri | 0:31f7be68e52d | 100 | fprintf(logp, text); |
chrigelburri | 0:31f7be68e52d | 101 | } |
chrigelburri | 0:31f7be68e52d | 102 | |
chrigelburri | 0:31f7be68e52d | 103 | void State::closePlotFile(void) |
chrigelburri | 0:31f7be68e52d | 104 | { |
chrigelburri | 0:31f7be68e52d | 105 | fclose(logp); |
chrigelburri | 0:31f7be68e52d | 106 | } |
chrigelburri | 0:31f7be68e52d | 107 | |
chrigelburri | 0:31f7be68e52d | 108 | float State::readBattery() |
chrigelburri | 0:31f7be68e52d | 109 | { |
chrigelburri | 0:31f7be68e52d | 110 | return battery->read()*BAT_MULTIPLICATOR; |
chrigelburri | 0:31f7be68e52d | 111 | } |
chrigelburri | 0:31f7be68e52d | 112 | |
chrigelburri | 0:31f7be68e52d | 113 | void State::setBatteryBit() |
chrigelburri | 0:31f7be68e52d | 114 | { |
chrigelburri | 0:31f7be68e52d | 115 | if(s->voltageBattery < BAT_MIN) { |
chrigelburri | 0:31f7be68e52d | 116 | s->state |= STATE_UNDER; |
chrigelburri | 0:31f7be68e52d | 117 | } else { |
chrigelburri | 0:31f7be68e52d | 118 | s->state &= (~STATE_UNDER); |
chrigelburri | 0:31f7be68e52d | 119 | } |
chrigelburri | 0:31f7be68e52d | 120 | } |
chrigelburri | 0:31f7be68e52d | 121 | |
chrigelburri | 0:31f7be68e52d | 122 | void State::setEnableLeftBit() |
chrigelburri | 0:31f7be68e52d | 123 | { |
chrigelburri | 0:31f7be68e52d | 124 | if(motorControllerLeft->isEnabled()) { |
chrigelburri | 0:31f7be68e52d | 125 | s->state &= (~STATE_LEFT); |
chrigelburri | 0:31f7be68e52d | 126 | } else { |
chrigelburri | 0:31f7be68e52d | 127 | s->state |= STATE_LEFT; |
chrigelburri | 0:31f7be68e52d | 128 | } |
chrigelburri | 0:31f7be68e52d | 129 | } |
chrigelburri | 0:31f7be68e52d | 130 | |
chrigelburri | 0:31f7be68e52d | 131 | void State::setEnableRightBit() |
chrigelburri | 0:31f7be68e52d | 132 | { |
chrigelburri | 0:31f7be68e52d | 133 | if(motorControllerRight->isEnabled()) { |
chrigelburri | 0:31f7be68e52d | 134 | s->state &= (~STATE_RIGHT); |
chrigelburri | 0:31f7be68e52d | 135 | } else { |
chrigelburri | 0:31f7be68e52d | 136 | s->state |= STATE_RIGHT; |
chrigelburri | 0:31f7be68e52d | 137 | } |
chrigelburri | 0:31f7be68e52d | 138 | } |
chrigelburri | 0:31f7be68e52d | 139 | |
chrigelburri | 0:31f7be68e52d | 140 | void State::startTimerFromZero() |
chrigelburri | 0:31f7be68e52d | 141 | { |
chrigelburri | 0:31f7be68e52d | 142 | timer.reset(); |
chrigelburri | 0:31f7be68e52d | 143 | timer.start(); |
chrigelburri | 0:31f7be68e52d | 144 | } |
chrigelburri | 0:31f7be68e52d | 145 | |
chrigelburri | 0:31f7be68e52d | 146 | void State::run() |
chrigelburri | 0:31f7be68e52d | 147 | { |
chrigelburri | 0:31f7be68e52d | 148 | s->millis = timer.read_ms(); |
chrigelburri | 0:31f7be68e52d | 149 | s->voltageBattery = readBattery(); |
chrigelburri | 0:31f7be68e52d | 150 | s->leftPulses = - motorControllerLeft->getPulses(); |
chrigelburri | 0:31f7be68e52d | 151 | s->rightPulses = motorControllerRight->getPulses(); |
chrigelburri | 0:31f7be68e52d | 152 | s->leftVelocity = motorControllerLeft->getActualSpeed() * 2.0f * WHEEL_RADIUS * PI; |
chrigelburri | 0:31f7be68e52d | 153 | s->rightVelocity = - motorControllerRight->getActualSpeed()* 2.0f * WHEEL_RADIUS * PI; |
chrigelburri | 0:31f7be68e52d | 154 | s->velocity = robotControl->getActualSpeed(); |
chrigelburri | 0:31f7be68e52d | 155 | s->omega = robotControl->getActualOmega(); |
chrigelburri | 0:31f7be68e52d | 156 | s->xAxis = robotControl->getxActualPosition(); |
chrigelburri | 0:31f7be68e52d | 157 | s->yAxis = robotControl->getyActualPosition(); |
chrigelburri | 0:31f7be68e52d | 158 | s->angle = robotControl->getActualTheta() * 180 / PI; |
chrigelburri | 0:31f7be68e52d | 159 | s->xAxisError = robotControl->getxPositionError(); |
chrigelburri | 0:31f7be68e52d | 160 | s->yAxisError = robotControl->getyPositionError(); |
chrigelburri | 0:31f7be68e52d | 161 | s->angleError = robotControl->getThetaError() * 180 / PI; |
chrigelburri | 2:d8e1613dc38b | 162 | // s->compassAngle = compass->getFilteredAngle() * 180 / PI; |
chrigelburri | 0:31f7be68e52d | 163 | |
chrigelburri | 2:d8e1613dc38b | 164 | s->rho = robotControl->getDistanceError(); |
chrigelburri | 2:d8e1613dc38b | 165 | s->lamda = robotControl->getThetaErrorToGoal() * 180 / PI; |
chrigelburri | 2:d8e1613dc38b | 166 | s->delta = robotControl->getThetaGoal() * 180 / PI; |
chrigelburri | 0:31f7be68e52d | 167 | |
chrigelburri | 0:31f7be68e52d | 168 | setBatteryBit(); |
chrigelburri | 0:31f7be68e52d | 169 | setEnableLeftBit(); |
chrigelburri | 0:31f7be68e52d | 170 | setEnableRightBit(); |
chrigelburri | 0:31f7be68e52d | 171 | |
chrigelburri | 0:31f7be68e52d | 172 | } |