David Kester / commands

working commands. singleton deleted

Dependents:   GonioTrainer

TrainingCommand.cpp@5:47cb37923f58, 2015-06-11 (annotated)

Committer:
dkester
Date:
Thu Jun 11 20:57:59 2015 +0000
Revision:
5:47cb37923f58
Parent:
4:f81029197ab2 
working 11 jun

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dkester 0:e188325211af 1 #include "TrainingCommand.h"
dkester 0:e188325211af 2
dkester 4:f81029197ab2 3 int16_t dummyAngle = 0;
dkester 1:fd4c0e2decb8 4
dkester 5:47cb37923f58 5 DigitalOut blue(p15);
dkester 5:47cb37923f58 6 DigitalOut data(p13);
dkester 5:47cb37923f58 7
dkester 4:f81029197ab2 8 TrainingCommand::TrainingCommand(Sensors* sensors, GonioService* gonioService)
dkester 2:c9e47ac47edb 9 {
dkester 2:c9e47ac47edb 10 this->sensors = sensors;
dkester 4:f81029197ab2 11 this->gonioService = gonioService;
dkester 5:47cb37923f58 12 blue = 0;
dkester 5:47cb37923f58 13 data = 0;
dkester 5:47cb37923f58 14
dkester 2:c9e47ac47edb 15 }
dkester 0:e188325211af 16
dkester 2:c9e47ac47edb 17 void TrainingCommand::initialize()
dkester 5:47cb37923f58 18 {
dkester 4:f81029197ab2 19 dummyAngle = 1;
dkester 3:32afe87d4b62 20 setTicker(0.1);
dkester 2:c9e47ac47edb 21 }
dkester 0:e188325211af 22
dkester 0:e188325211af 23
dkester 2:c9e47ac47edb 24 void TrainingCommand::execute()
dkester 2:c9e47ac47edb 25 {
dkester 4:f81029197ab2 26 /*
dkester 4:f81029197ab2 27 int8_t peak;
dkester 3:32afe87d4b62 28 int8_t readInt = sensors->getInterruptStatus();
dkester 4:f81029197ab2 29 int8_t statusInt = sensors->getMotionDetectionStatus();
dkester 4:f81029197ab2 30
dkester 3:32afe87d4b62 31 if((((readInt >> 5) & 0x1)+ ((readInt >> 6) & 0x1)+ ((readInt >> 7))&0x1) != 0) {
dkester 4:f81029197ab2 32 peak = 1;
dkester 3:32afe87d4b62 33 if((readInt >> 5) & 0x1) {
dkester 4:f81029197ab2 34 //printf(" ZERO MOTION ");
dkester 3:32afe87d4b62 35 }
dkester 3:32afe87d4b62 36 if((readInt >> 6) & 0x1) {
dkester 4:f81029197ab2 37 //printf(" MOTION DETECTION ");
dkester 3:32afe87d4b62 38 }
dkester 3:32afe87d4b62 39 if((readInt >> 7) & 0x1) {
dkester 4:f81029197ab2 40 //printf(" FREE FALL ");
dkester 3:32afe87d4b62 41 }
dkester 4:f81029197ab2 42 //printf("status: %d\n", statusInt);
dkester 5:47cb37923f58 43 }
dkester 3:32afe87d4b62 44
dkester 4:f81029197ab2 45 if (readInt & 0x01) {
dkester 4:f81029197ab2 46 //sensors->updateAngle();
dkester 4:f81029197ab2 47 //int16_t angle = (sensors->getAngle(0) << 8) + sensors->getAngle(1);
dkester 4:f81029197ab2 48 //printf("%d,%d\n", dummyAngle, peak);
dkester 4:f81029197ab2 49 gonioService->updateGonio(dummyAngle,0xffff,peak);
dkester 4:f81029197ab2 50 dummyAngle++;
dkester 4:f81029197ab2 51 peak = 0;
dkester 4:f81029197ab2 52 }
dkester 4:f81029197ab2 53 */
dkester 5:47cb37923f58 54
dkester 5:47cb37923f58 55 data = 1;
dkester 5:47cb37923f58 56 sensors->updateAngle();
dkester 5:47cb37923f58 57 sensors->updateIMU();
dkester 5:47cb37923f58 58
dkester 5:47cb37923f58 59 int16_t angle = (sensors->getAngle(0) << 8) + sensors->getAngle(1);
dkester 5:47cb37923f58 60 //int16_t angle = (sensors->getAngleDummy(0) << 8) + sensors->getAngleDummy(1);
dkester 5:47cb37923f58 61 //__disable_irq();
dkester 5:47cb37923f58 62 data = 0;
dkester 3:32afe87d4b62 63
dkester 5:47cb37923f58 64 wait(0.001);
dkester 5:47cb37923f58 65
dkester 5:47cb37923f58 66 if(gonioService->isConnected()) {
dkester 5:47cb37923f58 67 blue = 1;
dkester 5:47cb37923f58 68 gonioService->updateGonio(angle,angle,angle);
dkester 5:47cb37923f58 69 blue = 0;
dkester 5:47cb37923f58 70 }
dkester 5:47cb37923f58 71
dkester 5:47cb37923f58 72 //__enable_irq();
dkester 2:c9e47ac47edb 73 }
dkester 2:c9e47ac47edb 74
dkester 2:c9e47ac47edb 75 void TrainingCommand::button()
dkester 2:c9e47ac47edb 76 {
dkester 3:32afe87d4b62 77 detachTicker();
dkester 4:f81029197ab2 78 setLed(1);
dkester 3:32afe87d4b62 79 sensors->wakeIMU();
dkester 0:e188325211af 80 printf("**** BUTTON: TrainingCommand ****\n");
dkester 2:c9e47ac47edb 81 }
dkester 2:c9e47ac47edb 82
dkester 2:c9e47ac47edb 83 void TrainingCommand::finish()
dkester 2:c9e47ac47edb 84 {
dkester 3:32afe87d4b62 85 sensors->disableIMU();
dkester 0:e188325211af 86 setLed(0);
dkester 2:c9e47ac47edb 87 }
dkester 2:c9e47ac47edb 88
dkester 2:c9e47ac47edb 89 void TrainingCommand::printSensors()
dkester 2:c9e47ac47edb 90 {
dkester 3:32afe87d4b62 91 sensors->updateIMU();
dkester 2:c9e47ac47edb 92
dkester 3:32afe87d4b62 93 int16_t accel[3] = {0,0,0};
dkester 3:32afe87d4b62 94 int16_t gyro[3] = {0,0,0};
dkester 4:f81029197ab2 95
dkester 2:c9e47ac47edb 96 accel[0] = (sensors->getIMU(0)<<8)+sensors->getIMU(1);
dkester 2:c9e47ac47edb 97 accel[1] = (sensors->getIMU(2)<<8)+sensors->getIMU(3);
dkester 2:c9e47ac47edb 98 accel[2] = (sensors->getIMU(4)<<8)+sensors->getIMU(5);
dkester 2:c9e47ac47edb 99 gyro[0] = (sensors->getIMU(6)<<8)+sensors->getIMU(7);
dkester 2:c9e47ac47edb 100 gyro[1] = (sensors->getIMU(8)<<8)+sensors->getIMU(9);
dkester 2:c9e47ac47edb 101 gyro[2] = (sensors->getIMU(10)<<8)+sensors->getIMU(11);
dkester 2:c9e47ac47edb 102
dkester 2:c9e47ac47edb 103 float xangle = 57.295*atan((float)accel[1]/ sqrt(pow((float)accel[2],2)+pow((float)accel[0],2)));
dkester 2:c9e47ac47edb 104 float yangle = 57.295*atan((float)-accel[0]/ sqrt(pow((float)accel[2],2)+pow((float)accel[1],2)));
dkester 2:c9e47ac47edb 105
dkester 3:32afe87d4b62 106 float xgyro = ((float)gyro[0])/16.4;
dkester 2:c9e47ac47edb 107
dkester 0:e188325211af 108 printf("Aceleration Angles: %f, %f \n", xangle, yangle);
dkester 2:c9e47ac47edb 109 printf("gyroscope x: %f\n",xgyro);
dkester 4:f81029197ab2 110
dkester 2:c9e47ac47edb 111 }