Sooner Competitive Robotics / Mbed 2 deprecated IEEE_14_Freescale

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of IEEE_14_Freescale by IEEE 2014 Mbed

gyroReader.cpp@33:03b0b66038e1, 2014-03-23 (annotated)

Committer:
sswatek
Date:
Sun Mar 23 02:23:29 2014 +0000
Revision:
33:03b0b66038e1
Parent:
29:1132155bc7da 
able to move along the row of waves and find the gap

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soonerbot 1:c28fac16a109 1 #include "gyroReader.h"
soonerbot 1:c28fac16a109 2
soonerbot 7:3b2cf7efe5d1 3 //keep track of our heading
soonerbot 1:c28fac16a109 4 void gyroReader::gyroUpkeep(){
soonerbot 1:c28fac16a109 5 int xdiff,ydiff,zdiff;
soonerbot 1:c28fac16a109 6 checkGyro(xdiff,ydiff,zdiff);
sswatek 28:c7e8d2db03f5 7 checkCompass();
soonerbot 1:c28fac16a109 8 gyroZcount+=zdiff+zcal;
soonerbot 1:c28fac16a109 9 gyroXcount+=xdiff+xcal;
soonerbot 1:c28fac16a109 10 polls++;
soonerbot 1:c28fac16a109 11 }
soonerbot 1:c28fac16a109 12
soonerbot 7:3b2cf7efe5d1 13 //starts up gyro, accel and magnetometer
soonerbot 1:c28fac16a109 14 int gyroReader::startAccel(){
soonerbot 1:c28fac16a109 15 char cmd[4];
soonerbot 8:c23cd84befa2 16 //char addr=0x32;//0b0011000;
soonerbot 3:a223b0bf8256 17 char gyroaddr=0xD6;
soonerbot 1:c28fac16a109 18 char magaddr=0x3C;
soonerbot 1:c28fac16a109 19 cmd[0]=0x20;
soonerbot 1:c28fac16a109 20 cmd[1]=0x2f;//0b00101111;
soonerbot 1:c28fac16a109 21 cmd[2]=0;
soonerbot 1:c28fac16a109 22 cmd[3]=0;
soonerbot 4:adc885f4ab75 23 //printf("%d\r\n",accel.write(addr,cmd,2));
soonerbot 1:c28fac16a109 24 cmd[1]=0x0f;
soonerbot 1:c28fac16a109 25 printf("%d\r\n",accel.write(gyroaddr,cmd,2));
soonerbot 1:c28fac16a109 26 printf("%X,%X,%X,%X\r\n",cmd[0],cmd[1],cmd[2],cmd[3]);
soonerbot 4:adc885f4ab75 27 cmd[0]=0x00;
soonerbot 4:adc885f4ab75 28 cmd[1]=0x04;//75 samples/sec
sswatek 28:c7e8d2db03f5 29 cmd[2]=0x80;//second to smallest possible range (+-1.9 gauss)
soonerbot 4:adc885f4ab75 30 cmd[3]=0x00;//continuous mode
soonerbot 1:c28fac16a109 31 //pc.printf("%d\r\n",accel.write(magaddr,cmd,2));
soonerbot 4:adc885f4ab75 32 DBGPRINT("%d\r\n",accel.write(magaddr,cmd,4));
soonerbot 1:c28fac16a109 33 return 1;
soonerbot 1:c28fac16a109 34 }
soonerbot 1:c28fac16a109 35
soonerbot 4:adc885f4ab75 36 float gyroReader::checkCompass(){
soonerbot 4:adc885f4ab75 37 char com[1];
soonerbot 4:adc885f4ab75 38 char cmd[6]={0,0,0,0,0,0};
soonerbot 4:adc885f4ab75 39 char addr=0x3C;//0b0011110;
soonerbot 4:adc885f4ab75 40 com[0]=0x03|0x80;
soonerbot 4:adc885f4ab75 41 accel.write(addr, com, 1,true);
soonerbot 4:adc885f4ab75 42 accel.read(addr, cmd, 6);
sswatek 28:c7e8d2db03f5 43 xmag=(signed short)((((unsigned char)cmd[0])<<8)+(unsigned char)cmd[1]);
sswatek 28:c7e8d2db03f5 44 ymag=(signed short)((((unsigned char)cmd[2])<<8)+(unsigned char)cmd[3]);
sswatek 28:c7e8d2db03f5 45 zmag=(signed short)((((unsigned char)cmd[4])<<8)+(unsigned char)cmd[5]);
soonerbot 4:adc885f4ab75 46 //DBGPRINT("%d\t\t%d\t\t%d\r\n",xmag,ymag,zmag);
sswatek 28:c7e8d2db03f5 47 compDir = atan2(((double)xmag-xoffs)*xamp,((double)zmag-zoffs)*zamp);
soonerbot 4:adc885f4ab75 48 return 0;//atan2((double)xacc,(double)yacc);
soonerbot 4:adc885f4ab75 49 }
soonerbot 4:adc885f4ab75 50
soonerbot 7:3b2cf7efe5d1 51 // not useful at the moment
soonerbot 1:c28fac16a109 52 float gyroReader::checkAccel(){
soonerbot 1:c28fac16a109 53 char com[1];
soonerbot 1:c28fac16a109 54 char cmd[6]={0,0,0,0,0,0};
soonerbot 1:c28fac16a109 55 char addr=0x30;//0b0011000;
soonerbot 1:c28fac16a109 56 com[0]=0x28|0x80;
soonerbot 1:c28fac16a109 57 accel.write(addr, com, 1,true);
soonerbot 1:c28fac16a109 58 accel.read(addr, cmd, 6);
soonerbot 1:c28fac16a109 59 //int x=cmd
soonerbot 1:c28fac16a109 60 /*accel.start();
soonerbot 1:c28fac16a109 61 accel.write(addr);
soonerbot 1:c28fac16a109 62 accel.write(0x28|0x80);
soonerbot 1:c28fac16a109 63 accel.start();
soonerbot 1:c28fac16a109 64 accel.write(addr|1);
soonerbot 1:c28fac16a109 65 cmd[0]=accel.read(1);
soonerbot 1:c28fac16a109 66 cmd[1]=accel.read(1);
soonerbot 1:c28fac16a109 67 cmd[2]=accel.read(1);
soonerbot 1:c28fac16a109 68 cmd[3]=accel.read(0);
soonerbot 1:c28fac16a109 69 accel.stop();*/
soonerbot 1:c28fac16a109 70 //int yacc=(signed char)cmd[1];
soonerbot 1:c28fac16a109 71 //int xacc=(signed char)cmd[3];
soonerbot 1:c28fac16a109 72 //int zacc=-(signed char)cmd[5];
soonerbot 8:c23cd84befa2 73 //int xacc=(signed short)((((unsigned char)cmd[1])<<8)+(unsigned char)cmd[0]);
soonerbot 1:c28fac16a109 74 int yacc=(signed short)((((unsigned char)cmd[3])<<8)+(unsigned char)cmd[2]);
soonerbot 1:c28fac16a109 75 int zacc=(signed short)((((unsigned char)cmd[5])<<8)+(unsigned char)cmd[4]);
soonerbot 1:c28fac16a109 76 //int yacc=(signed char)cmd[3];
soonerbot 1:c28fac16a109 77 //int zacc=(signed char)cmd[5];
soonerbot 1:c28fac16a109 78 float angle=atan(zacc==0?yacc*1000000.0:(float)yacc/(float)zacc)-(zacc>=0?0.0:3.141592);
soonerbot 1:c28fac16a109 79 //pc.printf("%X\t%X\t%X\t%d\t%X\t%d\t%f\t%f\r\n",cmd[0],cmd[1],cmd[2],yacc,cmd[4],zacc,angle);
soonerbot 1:c28fac16a109 80 //pc.printf("%X\t%X\t%X\t%X\t%X\t%X\r\n",cmd[0],cmd[1],cmd[2],cmd[3],cmd[4],cmd[5]);
soonerbot 1:c28fac16a109 81 //pc.printf("%d\t\t%d\t\t%d\r\n",xacc,yacc,zacc);
soonerbot 1:c28fac16a109 82
soonerbot 1:c28fac16a109 83 addr=0x3D;//0b0011000;
soonerbot 1:c28fac16a109 84 com[0]=0x03|0x80;
soonerbot 1:c28fac16a109 85 accel.write(addr, com, 1,true);
soonerbot 1:c28fac16a109 86 accel.read(addr, cmd, 6);
soonerbot 8:c23cd84befa2 87 //xacc=(signed short)((((unsigned char)cmd[0])<<8)+(unsigned char)cmd[1]);
soonerbot 1:c28fac16a109 88 yacc=(signed short)((((unsigned char)cmd[2])<<8)+(unsigned char)cmd[3]);
soonerbot 1:c28fac16a109 89 zacc=(signed short)((((unsigned char)cmd[4])<<8)+(unsigned char)cmd[5]);
soonerbot 1:c28fac16a109 90 //pc.printf("%X\t%X\t%X\t%X\t%X\t%X\r\n",cmd[0],cmd[1],cmd[2],cmd[3],cmd[4],cmd[5]);
soonerbot 1:c28fac16a109 91 //pc.printf("%d\t\t%d\t\t%d\r\n",xacc,yacc,zacc);
soonerbot 1:c28fac16a109 92
soonerbot 1:c28fac16a109 93 return angle;
soonerbot 1:c28fac16a109 94 }
soonerbot 1:c28fac16a109 95
soonerbot 7:3b2cf7efe5d1 96 // checks how far the gyro has turned since the last check and passes back by reference
soonerbot 1:c28fac16a109 97 int gyroReader::checkGyro(int& xacc, int& yacc, int& zacc){
soonerbot 1:c28fac16a109 98 char com[1];
soonerbot 1:c28fac16a109 99 char cmd[6]={0,0,0,0,0,0};
soonerbot 3:a223b0bf8256 100 char addr=0xD6;//0b0011000;
soonerbot 1:c28fac16a109 101 com[0]=0x28|0x80;
soonerbot 1:c28fac16a109 102 accel.write(addr, com, 1,true);
soonerbot 1:c28fac16a109 103 accel.read(addr, cmd, 6);
soonerbot 1:c28fac16a109 104 xacc=(signed short)((((unsigned char)cmd[1])<<8)+(unsigned char)cmd[0]);
soonerbot 1:c28fac16a109 105 yacc=(signed short)((((unsigned char)cmd[3])<<8)+(unsigned char)cmd[2]);
soonerbot 1:c28fac16a109 106 zacc=(signed short)((((unsigned char)cmd[5])<<8)+(unsigned char)cmd[4]);
soonerbot 1:c28fac16a109 107 //pc.printf("%d\t%d\t%d\t%d\t%d\t%d\t%f\r\n",cmd[1],xacc,cmd[3],yacc,cmd[5],zacc);
soonerbot 1:c28fac16a109 108 return xacc;
soonerbot 1:c28fac16a109 109 }
soonerbot 1:c28fac16a109 110
soonerbot 7:3b2cf7efe5d1 111 //initalizes counters and calibration, also starts polling the gyro
soonerbot 1:c28fac16a109 112 gyroReader::gyroReader(PinName pinA, PinName pinB) : accel(pinA,pinB)/*, upkeepTimer(gyroReader::gyroUpkeep, osTimerPeriodic, (void *)0), testPin(test)*/{
soonerbot 1:c28fac16a109 113 gyroZcount=0;
soonerbot 1:c28fac16a109 114 gyroXcount=0;
sswatek 33:03b0b66038e1 115 zcal=-5;
soonerbot 3:a223b0bf8256 116 xcal=120;
soonerbot 1:c28fac16a109 117 polls=0;
sswatek 29:1132155bc7da 118 xoffs = -45.10;
sswatek 29:1132155bc7da 119 zoffs = -158.90;
sswatek 29:1132155bc7da 120 xamp = 1.0/458.0;
sswatek 29:1132155bc7da 121 zamp = 1.0/598.0;
soonerbot 1:c28fac16a109 122 startAccel();
soonerbot 4:adc885f4ab75 123 xmag=ymag=zmag=0;
sswatek 28:c7e8d2db03f5 124 compDir=0;
soonerbot 1:c28fac16a109 125 gyroUpkeepTicker.attach_us(this,&gyroReader::gyroUpkeep,10000);
soonerbot 1:c28fac16a109 126 }
soonerbot 1:c28fac16a109 127 void gyroReader::setLevel(){
soonerbot 1:c28fac16a109 128 gyroXcount=0;
soonerbot 1:c28fac16a109 129 }
soonerbot 1:c28fac16a109 130
soonerbot 1:c28fac16a109 131 void gyroReader::resetZ(){
soonerbot 1:c28fac16a109 132 gyroZcount=0;
soonerbot 1:c28fac16a109 133 }
soonerbot 1:c28fac16a109 134
soonerbot 1:c28fac16a109 135 int gyroReader::getZ(){
soonerbot 1:c28fac16a109 136 return gyroZcount;
soonerbot 1:c28fac16a109 137 }
soonerbot 1:c28fac16a109 138 int gyroReader::getX(){
soonerbot 1:c28fac16a109 139 return gyroXcount;
soonerbot 1:c28fac16a109 140 }
soonerbot 1:c28fac16a109 141 void gyroReader::reset(){
soonerbot 1:c28fac16a109 142 gyroZcount=gyroXcount=polls=0;
soonerbot 1:c28fac16a109 143 }
soonerbot 1:c28fac16a109 144
soonerbot 7:3b2cf7efe5d1 145 //only works after a reset, could be set up to calibrate during a match, though, if there is a time we know we're stationary
soonerbot 7:3b2cf7efe5d1 146 //also, might want to use a float? the variance is bigger than 1, but I could imagine a float calibration factor helping
soonerbot 1:c28fac16a109 147 void gyroReader::calibrate(){
soonerbot 1:c28fac16a109 148 zcal=-(gyroZcount/polls)+zcal;
soonerbot 1:c28fac16a109 149 xcal=-(gyroXcount/polls)+xcal;
soonerbot 3:a223b0bf8256 150 DBGPRINT("cal: %d, %d (%d)\r\n",zcal,xcal,polls);
soonerbot 4:adc885f4ab75 151 }
soonerbot 4:adc885f4ab75 152
soonerbot 7:3b2cf7efe5d1 153 //compares a gyro heading with the gyro reading, and accounts for repeated rotations
soonerbot 4:adc885f4ab75 154 int gyroReader::compZ(int compval){
soonerbot 4:adc885f4ab75 155 int midval= (compval-gyroZcount)%4050000;
soonerbot 4:adc885f4ab75 156 if(midval > 2025000){
soonerbot 4:adc885f4ab75 157 midval-=4050000;
soonerbot 4:adc885f4ab75 158 }
soonerbot 4:adc885f4ab75 159 return midval;
soonerbot 4:adc885f4ab75 160 }
soonerbot 4:adc885f4ab75 161
soonerbot 7:3b2cf7efe5d1 162 //performs the ticks-degrees conversion before passing back
soonerbot 4:adc885f4ab75 163 double gyroReader::getZDegrees(){
soonerbot 4:adc885f4ab75 164 return gyroZcount*360.0/4050000.0;
soonerbot 4:adc885f4ab75 165 }
soonerbot 12:925f52da3ba9 166
soonerbot 12:925f52da3ba9 167 void gyroReader::start(){
soonerbot 12:925f52da3ba9 168 stop();
soonerbot 12:925f52da3ba9 169 gyroUpkeepTicker.attach_us(this,&gyroReader::gyroUpkeep,10000);
soonerbot 12:925f52da3ba9 170 }
soonerbot 12:925f52da3ba9 171
soonerbot 12:925f52da3ba9 172 void gyroReader::stop(){
soonerbot 12:925f52da3ba9 173 gyroUpkeepTicker.detach();
soonerbot 12:925f52da3ba9 174 }