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

Committer:
soonerbot
Date:
Fri Nov 22 21:28:52 2013 +0000
Revision:
8:c23cd84befa2
Parent:
7:3b2cf7efe5d1
Child:
12:925f52da3ba9
More comments and cleanup

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);
soonerbot 7:3b2cf7efe5d1 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
soonerbot 4:adc885f4ab75 29 cmd[2]=0xE0;//largest possible range (8.1 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);
soonerbot 4:adc885f4ab75 43 xmag=(signed short)((((unsigned char)cmd[1])<<8)+(unsigned char)cmd[0]);
soonerbot 4:adc885f4ab75 44 ymag=(signed short)((((unsigned char)cmd[3])<<8)+(unsigned char)cmd[2]);
soonerbot 4:adc885f4ab75 45 zmag=(signed short)((((unsigned char)cmd[5])<<8)+(unsigned char)cmd[4]);
soonerbot 4:adc885f4ab75 46 //DBGPRINT("%d\t\t%d\t\t%d\r\n",xmag,ymag,zmag);
soonerbot 4:adc885f4ab75 47 return 0;//atan2((double)xacc,(double)yacc);
soonerbot 4:adc885f4ab75 48 }
soonerbot 4:adc885f4ab75 49
soonerbot 7:3b2cf7efe5d1 50 // not useful at the moment
soonerbot 1:c28fac16a109 51 float gyroReader::checkAccel(){
soonerbot 1:c28fac16a109 52 char com[1];
soonerbot 1:c28fac16a109 53 char cmd[6]={0,0,0,0,0,0};
soonerbot 1:c28fac16a109 54 char addr=0x30;//0b0011000;
soonerbot 1:c28fac16a109 55 com[0]=0x28|0x80;
soonerbot 1:c28fac16a109 56 accel.write(addr, com, 1,true);
soonerbot 1:c28fac16a109 57 accel.read(addr, cmd, 6);
soonerbot 1:c28fac16a109 58 //int x=cmd
soonerbot 1:c28fac16a109 59 /*accel.start();
soonerbot 1:c28fac16a109 60 accel.write(addr);
soonerbot 1:c28fac16a109 61 accel.write(0x28|0x80);
soonerbot 1:c28fac16a109 62 accel.start();
soonerbot 1:c28fac16a109 63 accel.write(addr|1);
soonerbot 1:c28fac16a109 64 cmd[0]=accel.read(1);
soonerbot 1:c28fac16a109 65 cmd[1]=accel.read(1);
soonerbot 1:c28fac16a109 66 cmd[2]=accel.read(1);
soonerbot 1:c28fac16a109 67 cmd[3]=accel.read(0);
soonerbot 1:c28fac16a109 68 accel.stop();*/
soonerbot 1:c28fac16a109 69 //int yacc=(signed char)cmd[1];
soonerbot 1:c28fac16a109 70 //int xacc=(signed char)cmd[3];
soonerbot 1:c28fac16a109 71 //int zacc=-(signed char)cmd[5];
soonerbot 8:c23cd84befa2 72 //int xacc=(signed short)((((unsigned char)cmd[1])<<8)+(unsigned char)cmd[0]);
soonerbot 1:c28fac16a109 73 int yacc=(signed short)((((unsigned char)cmd[3])<<8)+(unsigned char)cmd[2]);
soonerbot 1:c28fac16a109 74 int zacc=(signed short)((((unsigned char)cmd[5])<<8)+(unsigned char)cmd[4]);
soonerbot 1:c28fac16a109 75 //int yacc=(signed char)cmd[3];
soonerbot 1:c28fac16a109 76 //int zacc=(signed char)cmd[5];
soonerbot 1:c28fac16a109 77 float angle=atan(zacc==0?yacc*1000000.0:(float)yacc/(float)zacc)-(zacc>=0?0.0:3.141592);
soonerbot 1:c28fac16a109 78 //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 79 //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 80 //pc.printf("%d\t\t%d\t\t%d\r\n",xacc,yacc,zacc);
soonerbot 1:c28fac16a109 81
soonerbot 1:c28fac16a109 82 addr=0x3D;//0b0011000;
soonerbot 1:c28fac16a109 83 com[0]=0x03|0x80;
soonerbot 1:c28fac16a109 84 accel.write(addr, com, 1,true);
soonerbot 1:c28fac16a109 85 accel.read(addr, cmd, 6);
soonerbot 8:c23cd84befa2 86 //xacc=(signed short)((((unsigned char)cmd[0])<<8)+(unsigned char)cmd[1]);
soonerbot 1:c28fac16a109 87 yacc=(signed short)((((unsigned char)cmd[2])<<8)+(unsigned char)cmd[3]);
soonerbot 1:c28fac16a109 88 zacc=(signed short)((((unsigned char)cmd[4])<<8)+(unsigned char)cmd[5]);
soonerbot 1:c28fac16a109 89 //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 90 //pc.printf("%d\t\t%d\t\t%d\r\n",xacc,yacc,zacc);
soonerbot 1:c28fac16a109 91
soonerbot 1:c28fac16a109 92 return angle;
soonerbot 1:c28fac16a109 93 }
soonerbot 1:c28fac16a109 94
soonerbot 7:3b2cf7efe5d1 95 // checks how far the gyro has turned since the last check and passes back by reference
soonerbot 1:c28fac16a109 96 int gyroReader::checkGyro(int& xacc, int& yacc, int& zacc){
soonerbot 1:c28fac16a109 97 char com[1];
soonerbot 1:c28fac16a109 98 char cmd[6]={0,0,0,0,0,0};
soonerbot 3:a223b0bf8256 99 char addr=0xD6;//0b0011000;
soonerbot 1:c28fac16a109 100 com[0]=0x28|0x80;
soonerbot 1:c28fac16a109 101 accel.write(addr, com, 1,true);
soonerbot 1:c28fac16a109 102 accel.read(addr, cmd, 6);
soonerbot 1:c28fac16a109 103 xacc=(signed short)((((unsigned char)cmd[1])<<8)+(unsigned char)cmd[0]);
soonerbot 1:c28fac16a109 104 yacc=(signed short)((((unsigned char)cmd[3])<<8)+(unsigned char)cmd[2]);
soonerbot 1:c28fac16a109 105 zacc=(signed short)((((unsigned char)cmd[5])<<8)+(unsigned char)cmd[4]);
soonerbot 1:c28fac16a109 106 //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 107 return xacc;
soonerbot 1:c28fac16a109 108 }
soonerbot 1:c28fac16a109 109
soonerbot 7:3b2cf7efe5d1 110 //initalizes counters and calibration, also starts polling the gyro
soonerbot 1:c28fac16a109 111 gyroReader::gyroReader(PinName pinA, PinName pinB) : accel(pinA,pinB)/*, upkeepTimer(gyroReader::gyroUpkeep, osTimerPeriodic, (void *)0), testPin(test)*/{
soonerbot 1:c28fac16a109 112 gyroZcount=0;
soonerbot 1:c28fac16a109 113 gyroXcount=0;
soonerbot 3:a223b0bf8256 114 zcal=-54;
soonerbot 3:a223b0bf8256 115 xcal=120;
soonerbot 1:c28fac16a109 116 polls=0;
soonerbot 1:c28fac16a109 117 startAccel();
soonerbot 4:adc885f4ab75 118 xmag=ymag=zmag=0;
soonerbot 1:c28fac16a109 119 gyroUpkeepTicker.attach_us(this,&gyroReader::gyroUpkeep,10000);
soonerbot 1:c28fac16a109 120 }
soonerbot 1:c28fac16a109 121 void gyroReader::setLevel(){
soonerbot 1:c28fac16a109 122 gyroXcount=0;
soonerbot 1:c28fac16a109 123 }
soonerbot 1:c28fac16a109 124
soonerbot 1:c28fac16a109 125 void gyroReader::resetZ(){
soonerbot 1:c28fac16a109 126 gyroZcount=0;
soonerbot 1:c28fac16a109 127 }
soonerbot 1:c28fac16a109 128
soonerbot 1:c28fac16a109 129 int gyroReader::getZ(){
soonerbot 1:c28fac16a109 130 return gyroZcount;
soonerbot 1:c28fac16a109 131 }
soonerbot 1:c28fac16a109 132 int gyroReader::getX(){
soonerbot 1:c28fac16a109 133 return gyroXcount;
soonerbot 1:c28fac16a109 134 }
soonerbot 1:c28fac16a109 135 void gyroReader::reset(){
soonerbot 1:c28fac16a109 136 gyroZcount=gyroXcount=polls=0;
soonerbot 1:c28fac16a109 137 }
soonerbot 1:c28fac16a109 138
soonerbot 7:3b2cf7efe5d1 139 //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 140 //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 141 void gyroReader::calibrate(){
soonerbot 1:c28fac16a109 142 zcal=-(gyroZcount/polls)+zcal;
soonerbot 1:c28fac16a109 143 xcal=-(gyroXcount/polls)+xcal;
soonerbot 3:a223b0bf8256 144 DBGPRINT("cal: %d, %d (%d)\r\n",zcal,xcal,polls);
soonerbot 4:adc885f4ab75 145 }
soonerbot 4:adc885f4ab75 146
soonerbot 7:3b2cf7efe5d1 147 //compares a gyro heading with the gyro reading, and accounts for repeated rotations
soonerbot 4:adc885f4ab75 148 int gyroReader::compZ(int compval){
soonerbot 4:adc885f4ab75 149 int midval= (compval-gyroZcount)%4050000;
soonerbot 4:adc885f4ab75 150 if(midval > 2025000){
soonerbot 4:adc885f4ab75 151 midval-=4050000;
soonerbot 4:adc885f4ab75 152 }
soonerbot 4:adc885f4ab75 153 return midval;
soonerbot 4:adc885f4ab75 154 }
soonerbot 4:adc885f4ab75 155
soonerbot 7:3b2cf7efe5d1 156 //performs the ticks-degrees conversion before passing back
soonerbot 4:adc885f4ab75 157 double gyroReader::getZDegrees(){
soonerbot 4:adc885f4ab75 158 return gyroZcount*360.0/4050000.0;
soonerbot 4:adc885f4ab75 159 }