David Kester / commands

working commands. singleton deleted

Dependents:   GonioTrainer

TrainingCommand.cpp@3:32afe87d4b62, 2015-06-07 (annotated)

Committer:
dkester
Date:
Sun Jun 07 21:57:46 2015 +0000
Revision:
3:32afe87d4b62
Parent:
2:c9e47ac47edb
Child:
4:f81029197ab2
working zondag 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 3:32afe87d4b62 13 setTicker(0.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 3:32afe87d4b62 19 //IMU
dkester 3:32afe87d4b62 20 printf("peak: ");
dkester 3:32afe87d4b62 21 int8_t readInt = sensors->getInterruptStatus();
dkester 3:32afe87d4b62 22 int8_t statusInt = sensors->readRegister(0x61);
dkester 3:32afe87d4b62 23
dkester 3:32afe87d4b62 24 //[7] MOT_XNEG [6] MOT_XPOS [5] MOT_YNEG [4] MOT_YPOS [3] MOT_ZNEG [2] MOT_ZPOS
dkester 3:32afe87d4b62 25
dkester 3:32afe87d4b62 26 if((((readInt >> 5) & 0x1)+ ((readInt >> 6) & 0x1)+ ((readInt >> 7))&0x1) != 0) {
dkester 3:32afe87d4b62 27 printf("1 =>");
dkester 3:32afe87d4b62 28 if((readInt >> 5) & 0x1) {
dkester 3:32afe87d4b62 29 printf(" ZERO MOTION ");
dkester 3:32afe87d4b62 30 }
dkester 3:32afe87d4b62 31 if((readInt >> 6) & 0x1) {
dkester 3:32afe87d4b62 32 printf(" MOTION DETECTION ");
dkester 3:32afe87d4b62 33 }
dkester 3:32afe87d4b62 34 if((readInt >> 7) & 0x1) {
dkester 3:32afe87d4b62 35 printf(" FREE FALL ");
dkester 3:32afe87d4b62 36 }
dkester 3:32afe87d4b62 37 printf("status: %d\n", statusInt);
dkester 3:32afe87d4b62 38 } else {printf("0\n");}
dkester 3:32afe87d4b62 39
dkester 3:32afe87d4b62 40 //printSensors();
dkester 3:32afe87d4b62 41
dkester 3:32afe87d4b62 42 //Angle
dkester 3:32afe87d4b62 43 sensors->updateAngle();
dkester 3:32afe87d4b62 44 int16_t angle = (sensors->getAngle(0) << 8) + sensors->getAngle(1);
dkester 3:32afe87d4b62 45 printf("angle: %.2f\n\n", ((float)angle) * 0.087912087);
dkester 3:32afe87d4b62 46
dkester 3:32afe87d4b62 47
dkester 2:c9e47ac47edb 48 }
dkester 2:c9e47ac47edb 49
dkester 2:c9e47ac47edb 50 void TrainingCommand::button()
dkester 2:c9e47ac47edb 51 {
dkester 3:32afe87d4b62 52 detachTicker();
dkester 3:32afe87d4b62 53 sensors->wakeIMU();
dkester 0:e188325211af 54 printf("**** BUTTON: TrainingCommand ****\n");
dkester 2:c9e47ac47edb 55 }
dkester 2:c9e47ac47edb 56
dkester 2:c9e47ac47edb 57 void TrainingCommand::finish()
dkester 2:c9e47ac47edb 58 {
dkester 3:32afe87d4b62 59 sensors->disableIMU();
dkester 0:e188325211af 60 setLed(0);
dkester 2:c9e47ac47edb 61 }
dkester 2:c9e47ac47edb 62
dkester 2:c9e47ac47edb 63 void TrainingCommand::printSensors()
dkester 2:c9e47ac47edb 64 {
dkester 3:32afe87d4b62 65 sensors->updateIMU();
dkester 2:c9e47ac47edb 66
dkester 3:32afe87d4b62 67 int16_t accel[3] = {0,0,0};
dkester 3:32afe87d4b62 68 int16_t gyro[3] = {0,0,0};
dkester 3:32afe87d4b62 69
dkester 2:c9e47ac47edb 70 accel[0] = (sensors->getIMU(0)<<8)+sensors->getIMU(1);
dkester 2:c9e47ac47edb 71 accel[1] = (sensors->getIMU(2)<<8)+sensors->getIMU(3);
dkester 2:c9e47ac47edb 72 accel[2] = (sensors->getIMU(4)<<8)+sensors->getIMU(5);
dkester 2:c9e47ac47edb 73 gyro[0] = (sensors->getIMU(6)<<8)+sensors->getIMU(7);
dkester 2:c9e47ac47edb 74 gyro[1] = (sensors->getIMU(8)<<8)+sensors->getIMU(9);
dkester 2:c9e47ac47edb 75 gyro[2] = (sensors->getIMU(10)<<8)+sensors->getIMU(11);
dkester 2:c9e47ac47edb 76
dkester 2:c9e47ac47edb 77 float xangle = 57.295*atan((float)accel[1]/ sqrt(pow((float)accel[2],2)+pow((float)accel[0],2)));
dkester 2:c9e47ac47edb 78 float yangle = 57.295*atan((float)-accel[0]/ sqrt(pow((float)accel[2],2)+pow((float)accel[1],2)));
dkester 2:c9e47ac47edb 79
dkester 3:32afe87d4b62 80 float xgyro = ((float)gyro[0])/16.4;
dkester 2:c9e47ac47edb 81
dkester 0:e188325211af 82 printf("Aceleration Angles: %f, %f \n", xangle, yangle);
dkester 2:c9e47ac47edb 83 printf("gyroscope x: %f\n",xgyro);
dkester 3:32afe87d4b62 84
dkester 2:c9e47ac47edb 85 }