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@7:3b2cf7efe5d1, 2013-11-22 (annotated)

Committer:
soonerbot
Date:
Fri Nov 22 18:00:18 2013 +0000
Revision:
7:3b2cf7efe5d1
Parent:
4:adc885f4ab75
Child:
8:c23cd84befa2
Commented most things

Who changed what in which revision?

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