working commands. singleton deleted
TrainingCommand.cpp@2:c9e47ac47edb, 2015-06-07 (annotated)
- Committer:
- dkester
- Date:
- Sun Jun 07 17:49:23 2015 +0000
- Revision:
- 2:c9e47ac47edb
- Parent:
- 1:fd4c0e2decb8
- Child:
- 3:32afe87d4b62
gonio trainer 7 juni
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dkester | 0:e188325211af | 1 | #include "TrainingCommand.h" |
dkester | 0:e188325211af | 2 | |
dkester | 1:fd4c0e2decb8 | 3 | |
dkester | 1:fd4c0e2decb8 | 4 | |
dkester | 2:c9e47ac47edb | 5 | TrainingCommand::TrainingCommand(Sensors* sensors) |
dkester | 2:c9e47ac47edb | 6 | { |
dkester | 2:c9e47ac47edb | 7 | this->sensors = sensors; |
dkester | 2:c9e47ac47edb | 8 | } |
dkester | 0:e188325211af | 9 | |
dkester | 2:c9e47ac47edb | 10 | void TrainingCommand::initialize() |
dkester | 2:c9e47ac47edb | 11 | { |
dkester | 0:e188325211af | 12 | printf("TrainingCommand\n"); |
dkester | 1:fd4c0e2decb8 | 13 | setLed(1); |
dkester | 2:c9e47ac47edb | 14 | } |
dkester | 0:e188325211af | 15 | |
dkester | 0:e188325211af | 16 | |
dkester | 2:c9e47ac47edb | 17 | void TrainingCommand::execute() |
dkester | 2:c9e47ac47edb | 18 | { |
dkester | 0:e188325211af | 19 | printSensors(); |
dkester | 2:c9e47ac47edb | 20 | } |
dkester | 2:c9e47ac47edb | 21 | |
dkester | 2:c9e47ac47edb | 22 | void TrainingCommand::button() |
dkester | 2:c9e47ac47edb | 23 | { |
dkester | 0:e188325211af | 24 | this->sensors->wakeIMU(); |
dkester | 0:e188325211af | 25 | printf("**** BUTTON: TrainingCommand ****\n"); |
dkester | 2:c9e47ac47edb | 26 | } |
dkester | 2:c9e47ac47edb | 27 | |
dkester | 2:c9e47ac47edb | 28 | void TrainingCommand::finish() |
dkester | 2:c9e47ac47edb | 29 | { |
dkester | 0:e188325211af | 30 | setLed(0); |
dkester | 0:e188325211af | 31 | this->sensors->disableIMU(); |
dkester | 2:c9e47ac47edb | 32 | } |
dkester | 2:c9e47ac47edb | 33 | |
dkester | 2:c9e47ac47edb | 34 | void TrainingCommand::printSensors() |
dkester | 2:c9e47ac47edb | 35 | { |
dkester | 2:c9e47ac47edb | 36 | sensors->updateIMUData(); |
dkester | 2:c9e47ac47edb | 37 | sensors->updateAngle(); |
dkester | 2:c9e47ac47edb | 38 | |
dkester | 2:c9e47ac47edb | 39 | int8_t accel[3] = {0,0,0}; |
dkester | 2:c9e47ac47edb | 40 | int8_t gyro[3] = {0,0,0};; |
dkester | 2:c9e47ac47edb | 41 | |
dkester | 2:c9e47ac47edb | 42 | accel[0] = (sensors->getIMU(0)<<8)+sensors->getIMU(1); |
dkester | 2:c9e47ac47edb | 43 | accel[1] = (sensors->getIMU(2)<<8)+sensors->getIMU(3); |
dkester | 2:c9e47ac47edb | 44 | accel[2] = (sensors->getIMU(4)<<8)+sensors->getIMU(5); |
dkester | 2:c9e47ac47edb | 45 | gyro[0] = (sensors->getIMU(6)<<8)+sensors->getIMU(7); |
dkester | 2:c9e47ac47edb | 46 | gyro[1] = (sensors->getIMU(8)<<8)+sensors->getIMU(9); |
dkester | 2:c9e47ac47edb | 47 | gyro[2] = (sensors->getIMU(10)<<8)+sensors->getIMU(11); |
dkester | 2:c9e47ac47edb | 48 | |
dkester | 2:c9e47ac47edb | 49 | float xangle = 57.295*atan((float)accel[1]/ sqrt(pow((float)accel[2],2)+pow((float)accel[0],2))); |
dkester | 2:c9e47ac47edb | 50 | float yangle = 57.295*atan((float)-accel[0]/ sqrt(pow((float)accel[2],2)+pow((float)accel[1],2))); |
dkester | 2:c9e47ac47edb | 51 | |
dkester | 2:c9e47ac47edb | 52 | float xgyro = ((float)gyro[0])/200; |
dkester | 2:c9e47ac47edb | 53 | |
dkester | 2:c9e47ac47edb | 54 | printf("angle: %.2f\n", ((float)(sensors->getAngle(0) + sensors->getAngle(1))) * 0.087912087); |
dkester | 0:e188325211af | 55 | printf("Aceleration Angles: %f, %f \n", xangle, yangle); |
dkester | 2:c9e47ac47edb | 56 | |
dkester | 2:c9e47ac47edb | 57 | printf("gyroscope x: %f\n",xgyro); |
dkester | 2:c9e47ac47edb | 58 | } |