![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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
State/State.cpp@24:08241be546ba, 2013-05-19 (annotated)
- Committer:
- chrigelburri
- Date:
- Sun May 19 14:54:49 2013 +0000
- Revision:
- 24:08241be546ba
- Parent:
- 12:235e318a414f
- Child:
- 27:a13ede88e75f
- Child:
- 29:937c74ff040e
LED lauft :-) cool
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 | 11:775ebb69d5e1 | 11 | State::State(state_t* s, |
chrigelburri | 11:775ebb69d5e1 | 12 | RobotControl* robotControl, |
chrigelburri | 11:775ebb69d5e1 | 13 | MaxonESCON* motorControllerLeft, |
chrigelburri | 11:775ebb69d5e1 | 14 | MaxonESCON* motorControllerRight, |
chrigelburri | 11:775ebb69d5e1 | 15 | PinName batteryPin, |
chrigelburri | 11:775ebb69d5e1 | 16 | float period) |
chrigelburri | 11:775ebb69d5e1 | 17 | : Task(period) , |
chrigelburri | 11:775ebb69d5e1 | 18 | battery(batteryPin) |
chrigelburri | 0:31f7be68e52d | 19 | { |
chrigelburri | 0:31f7be68e52d | 20 | /* get peripherals */ |
chrigelburri | 0:31f7be68e52d | 21 | this->s = s; |
chrigelburri | 0:31f7be68e52d | 22 | this->robotControl = robotControl; |
chrigelburri | 0:31f7be68e52d | 23 | this->motorControllerLeft = motorControllerLeft; |
chrigelburri | 0:31f7be68e52d | 24 | this->motorControllerRight = motorControllerRight; |
chrigelburri | 0:31f7be68e52d | 25 | this->battery =battery; |
chrigelburri | 0:31f7be68e52d | 26 | this->period = period; |
chrigelburri | 0:31f7be68e52d | 27 | } |
chrigelburri | 0:31f7be68e52d | 28 | |
chrigelburri | 0:31f7be68e52d | 29 | State::~State() {} |
chrigelburri | 0:31f7be68e52d | 30 | |
chrigelburri | 0:31f7be68e52d | 31 | void State::initPlotFile(void) |
chrigelburri | 0:31f7be68e52d | 32 | { |
chrigelburri | 0:31f7be68e52d | 33 | logp = fopen("/local/plots.txt", "w"); // only write |
chrigelburri | 0:31f7be68e52d | 34 | if(logp == NULL) { |
chrigelburri | 0:31f7be68e52d | 35 | exit(1); |
chrigelburri | 0:31f7be68e52d | 36 | } else { |
chrigelburri | 0:31f7be68e52d | 37 | fprintf(logp, |
chrigelburri | 0:31f7be68e52d | 38 | "Time[ms]\t" |
chrigelburri | 0:31f7be68e52d | 39 | "BatteryVoltage[V]\t" |
chrigelburri | 0:31f7be68e52d | 40 | "NumberOfPulsesLeft\t" |
chrigelburri | 0:31f7be68e52d | 41 | "NumberOfPulsesRight\t" |
chrigelburri | 0:31f7be68e52d | 42 | "VelocityLeft[m/s]\t" |
chrigelburri | 0:31f7be68e52d | 43 | "VelocityRight[m/s]\t" |
chrigelburri | 4:3a97923ff2d4 | 44 | "VelocityCar[m/s]\t" //7 |
chrigelburri | 0:31f7be68e52d | 45 | "VelocityRotation[grad/s]\t" |
chrigelburri | 0:31f7be68e52d | 46 | "X-AxisCo-ordinate[m]\t" |
chrigelburri | 0:31f7be68e52d | 47 | "Y-AxisCo-ordinate[m]\t" |
chrigelburri | 0:31f7be68e52d | 48 | "X-AxisError[m]\t" |
chrigelburri | 0:31f7be68e52d | 49 | "X-AxisError[m]\t" |
chrigelburri | 0:31f7be68e52d | 50 | "AngleError[grad]\t" |
chrigelburri | 0:31f7be68e52d | 51 | "AngleCar[grad]\t" |
chrigelburri | 5:48a258f6335e | 52 | "SetpointX-Axis[m]\t" //15 |
chrigelburri | 0:31f7be68e52d | 53 | "SetpointY-Axis[m]\t" |
chrigelburri | 5:48a258f6335e | 54 | "SetpointAngel[grad]\t" //17 |
chrigelburri | 8:696c2f9dfc62 | 55 | "SetpointVelocitiy[m/s]\t" |
chrigelburri | 8:696c2f9dfc62 | 56 | "SetpointVelocitiyRotations[rad/s]\t" |
chrigelburri | 11:775ebb69d5e1 | 57 | "State\t" //20 |
chrigelburri | 11:775ebb69d5e1 | 58 | "distanceToGoal[m]\t" //21 |
chrigelburri | 5:48a258f6335e | 59 | "angleToGoal[grad]\t" |
chrigelburri | 5:48a258f6335e | 60 | "thetaFromTheGoal[grad]\n"); |
chrigelburri | 0:31f7be68e52d | 61 | } |
chrigelburri | 0:31f7be68e52d | 62 | } |
chrigelburri | 0:31f7be68e52d | 63 | |
chrigelburri | 0:31f7be68e52d | 64 | |
chrigelburri | 0:31f7be68e52d | 65 | void State::savePlotFile(state_t s) |
chrigelburri | 0:31f7be68e52d | 66 | { |
chrigelburri | 0:31f7be68e52d | 67 | char buf[256]; |
chrigelburri | 0:31f7be68e52d | 68 | |
chrigelburri | 6:48eeb41188dd | 69 | 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 | 11:775ebb69d5e1 | 70 | "%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 | 71 | s.millis, |
chrigelburri | 0:31f7be68e52d | 72 | s.voltageBattery, |
chrigelburri | 0:31f7be68e52d | 73 | s.leftPulses, |
chrigelburri | 0:31f7be68e52d | 74 | s.rightPulses, |
chrigelburri | 0:31f7be68e52d | 75 | s.leftVelocity, |
chrigelburri | 0:31f7be68e52d | 76 | s.rightVelocity, |
chrigelburri | 0:31f7be68e52d | 77 | s.velocity, |
chrigelburri | 0:31f7be68e52d | 78 | s.omega, |
chrigelburri | 0:31f7be68e52d | 79 | s.xAxis, |
chrigelburri | 0:31f7be68e52d | 80 | s.yAxis, |
chrigelburri | 0:31f7be68e52d | 81 | s.xAxisError, |
chrigelburri | 0:31f7be68e52d | 82 | s.yAxisError, |
chrigelburri | 0:31f7be68e52d | 83 | s.angleError, |
chrigelburri | 0:31f7be68e52d | 84 | s.angle, |
chrigelburri | 0:31f7be68e52d | 85 | s.setxAxis, |
chrigelburri | 0:31f7be68e52d | 86 | s.setyAxis, |
chrigelburri | 0:31f7be68e52d | 87 | s.setAngle, |
chrigelburri | 8:696c2f9dfc62 | 88 | s.setVelocity, |
chrigelburri | 8:696c2f9dfc62 | 89 | s.setOmega, |
chrigelburri | 2:d8e1613dc38b | 90 | s.state, |
chrigelburri | 2:d8e1613dc38b | 91 | s.rho, |
chrigelburri | 2:d8e1613dc38b | 92 | s.lamda, |
chrigelburri | 2:d8e1613dc38b | 93 | s.delta |
chrigelburri | 0:31f7be68e52d | 94 | ); |
chrigelburri | 0:31f7be68e52d | 95 | |
chrigelburri | 0:31f7be68e52d | 96 | if (logp) |
chrigelburri | 0:31f7be68e52d | 97 | fprintf(logp, buf); |
chrigelburri | 0:31f7be68e52d | 98 | fprintf(logp, "\n"); // new line |
chrigelburri | 0:31f7be68e52d | 99 | } |
chrigelburri | 0:31f7be68e52d | 100 | |
chrigelburri | 0:31f7be68e52d | 101 | void State::closePlotFile(void) |
chrigelburri | 0:31f7be68e52d | 102 | { |
chrigelburri | 0:31f7be68e52d | 103 | fclose(logp); |
chrigelburri | 0:31f7be68e52d | 104 | } |
chrigelburri | 0:31f7be68e52d | 105 | |
chrigelburri | 0:31f7be68e52d | 106 | float State::readBattery() |
chrigelburri | 0:31f7be68e52d | 107 | { |
chrigelburri | 11:775ebb69d5e1 | 108 | return battery.read()*BAT_MULTIPLICATOR; |
chrigelburri | 0:31f7be68e52d | 109 | } |
chrigelburri | 0:31f7be68e52d | 110 | |
chrigelburri | 0:31f7be68e52d | 111 | void State::setBatteryBit() |
chrigelburri | 0:31f7be68e52d | 112 | { |
chrigelburri | 0:31f7be68e52d | 113 | if(s->voltageBattery < BAT_MIN) { |
chrigelburri | 0:31f7be68e52d | 114 | s->state |= STATE_UNDER; |
chrigelburri | 0:31f7be68e52d | 115 | } else { |
chrigelburri | 0:31f7be68e52d | 116 | s->state &= (~STATE_UNDER); |
chrigelburri | 0:31f7be68e52d | 117 | } |
chrigelburri | 0:31f7be68e52d | 118 | } |
chrigelburri | 0:31f7be68e52d | 119 | |
chrigelburri | 0:31f7be68e52d | 120 | void State::setEnableLeftBit() |
chrigelburri | 0:31f7be68e52d | 121 | { |
chrigelburri | 0:31f7be68e52d | 122 | if(motorControllerLeft->isEnabled()) { |
chrigelburri | 0:31f7be68e52d | 123 | s->state &= (~STATE_LEFT); |
chrigelburri | 0:31f7be68e52d | 124 | } else { |
chrigelburri | 0:31f7be68e52d | 125 | s->state |= STATE_LEFT; |
chrigelburri | 0:31f7be68e52d | 126 | } |
chrigelburri | 0:31f7be68e52d | 127 | } |
chrigelburri | 0:31f7be68e52d | 128 | |
chrigelburri | 0:31f7be68e52d | 129 | void State::setEnableRightBit() |
chrigelburri | 0:31f7be68e52d | 130 | { |
chrigelburri | 0:31f7be68e52d | 131 | if(motorControllerRight->isEnabled()) { |
chrigelburri | 0:31f7be68e52d | 132 | s->state &= (~STATE_RIGHT); |
chrigelburri | 0:31f7be68e52d | 133 | } else { |
chrigelburri | 0:31f7be68e52d | 134 | s->state |= STATE_RIGHT; |
chrigelburri | 0:31f7be68e52d | 135 | } |
chrigelburri | 0:31f7be68e52d | 136 | } |
chrigelburri | 0:31f7be68e52d | 137 | |
chrigelburri | 0:31f7be68e52d | 138 | void State::startTimerFromZero() |
chrigelburri | 0:31f7be68e52d | 139 | { |
chrigelburri | 0:31f7be68e52d | 140 | timer.reset(); |
chrigelburri | 0:31f7be68e52d | 141 | timer.start(); |
chrigelburri | 0:31f7be68e52d | 142 | } |
chrigelburri | 0:31f7be68e52d | 143 | |
chrigelburri | 24:08241be546ba | 144 | float State::dim( int idx, float f ) |
chrigelburri | 24:08241be546ba | 145 | { |
chrigelburri | 24:08241be546ba | 146 | return (sin(f + float(idx)* 1.0) + 1.0) / 2.0; // transform value from -1.0 .. 1.0 to 0.0 .. 1.0 |
chrigelburri | 24:08241be546ba | 147 | } |
chrigelburri | 24:08241be546ba | 148 | |
chrigelburri | 0:31f7be68e52d | 149 | void State::run() |
chrigelburri | 0:31f7be68e52d | 150 | { |
chrigelburri | 0:31f7be68e52d | 151 | s->millis = timer.read_ms(); |
chrigelburri | 0:31f7be68e52d | 152 | s->voltageBattery = readBattery(); |
chrigelburri | 0:31f7be68e52d | 153 | s->leftPulses = - motorControllerLeft->getPulses(); |
chrigelburri | 0:31f7be68e52d | 154 | s->rightPulses = motorControllerRight->getPulses(); |
chrigelburri | 6:48eeb41188dd | 155 | s->leftVelocity = motorControllerLeft->getActualSpeed() * |
chrigelburri | 8:696c2f9dfc62 | 156 | 2.0f * WHEEL_RADIUS_LEFT * PI * GEAR; |
chrigelburri | 6:48eeb41188dd | 157 | s->rightVelocity = - motorControllerRight->getActualSpeed()* |
chrigelburri | 8:696c2f9dfc62 | 158 | 2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR; |
chrigelburri | 0:31f7be68e52d | 159 | s->velocity = robotControl->getActualSpeed(); |
chrigelburri | 0:31f7be68e52d | 160 | s->omega = robotControl->getActualOmega(); |
chrigelburri | 0:31f7be68e52d | 161 | s->xAxis = robotControl->getxActualPosition(); |
chrigelburri | 0:31f7be68e52d | 162 | s->yAxis = robotControl->getyActualPosition(); |
chrigelburri | 0:31f7be68e52d | 163 | s->angle = robotControl->getActualTheta() * 180 / PI; |
chrigelburri | 0:31f7be68e52d | 164 | s->xAxisError = robotControl->getxPositionError(); |
chrigelburri | 0:31f7be68e52d | 165 | s->yAxisError = robotControl->getyPositionError(); |
chrigelburri | 0:31f7be68e52d | 166 | s->angleError = robotControl->getThetaError() * 180 / PI; |
chrigelburri | 5:48a258f6335e | 167 | s->setxAxis = robotControl->getDesiredxPosition(); |
chrigelburri | 5:48a258f6335e | 168 | s->setyAxis = robotControl->getDesiredyPosition(); |
chrigelburri | 5:48a258f6335e | 169 | s->setAngle = robotControl->getDesiredTheta() * 180 / PI; |
chrigelburri | 8:696c2f9dfc62 | 170 | s->setVelocity = robotControl->getDesiredSpeed(); |
chrigelburri | 8:696c2f9dfc62 | 171 | s->setOmega = robotControl->getDesiredOmega(); |
chrigelburri | 0:31f7be68e52d | 172 | |
chrigelburri | 2:d8e1613dc38b | 173 | s->rho = robotControl->getDistanceError(); |
chrigelburri | 2:d8e1613dc38b | 174 | s->lamda = robotControl->getThetaErrorToGoal() * 180 / PI; |
chrigelburri | 2:d8e1613dc38b | 175 | s->delta = robotControl->getThetaGoal() * 180 / PI; |
chrigelburri | 0:31f7be68e52d | 176 | |
chrigelburri | 0:31f7be68e52d | 177 | setBatteryBit(); |
chrigelburri | 0:31f7be68e52d | 178 | setEnableLeftBit(); |
chrigelburri | 0:31f7be68e52d | 179 | setEnableRightBit(); |
chrigelburri | 0:31f7be68e52d | 180 | } |