David Kester / commands

working commands. singleton deleted

Dependents:   GonioTrainer

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?

UserRevisionLine numberNew 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 }