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@4:adc885f4ab75, 2013-11-17 (annotated)

Committer:
soonerbot
Date:
Sun Nov 17 21:04:47 2013 +0000
Revision:
4:adc885f4ab75
Parent:
3:a223b0bf8256
Child:
7:3b2cf7efe5d1
Gyro working, compass giving values, and fiddling with control systems for driving forward/back

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 1:c28fac16a109 4 void gyroReader::gyroUpkeep(){
soonerbot 1:c28fac16a109 5 //testPin=1;
soonerbot 1:c28fac16a109 6 int xdiff,ydiff,zdiff;
soonerbot 1:c28fac16a109 7 checkGyro(xdiff,ydiff,zdiff);
soonerbot 4:adc885f4ab75 8 checkCompass();
soonerbot 1:c28fac16a109 9 gyroZcount+=zdiff+zcal;
soonerbot 1:c28fac16a109 10 gyroXcount+=xdiff+xcal;
soonerbot 1:c28fac16a109 11 polls++;
soonerbot 1:c28fac16a109 12 //testPin=0;
soonerbot 1:c28fac16a109 13 }
soonerbot 1:c28fac16a109 14
soonerbot 1:c28fac16a109 15 int gyroReader::startAccel(){
soonerbot 1:c28fac16a109 16 char cmd[4];
soonerbot 4:adc885f4ab75 17 char addr=0x32;//0b0011000;
soonerbot 3:a223b0bf8256 18 char gyroaddr=0xD6;
soonerbot 1:c28fac16a109 19 char magaddr=0x3C;
soonerbot 1:c28fac16a109 20 cmd[0]=0x20;
soonerbot 1:c28fac16a109 21 cmd[1]=0x2f;//0b00101111;
soonerbot 1:c28fac16a109 22 cmd[2]=0;
soonerbot 1:c28fac16a109 23 cmd[3]=0;
soonerbot 4:adc885f4ab75 24 //printf("%d\r\n",accel.write(addr,cmd,2));
soonerbot 1:c28fac16a109 25 cmd[1]=0x0f;
soonerbot 1:c28fac16a109 26 printf("%d\r\n",accel.write(gyroaddr,cmd,2));
soonerbot 1:c28fac16a109 27 printf("%X,%X,%X,%X\r\n",cmd[0],cmd[1],cmd[2],cmd[3]);
soonerbot 4:adc885f4ab75 28 cmd[0]=0x00;
soonerbot 4:adc885f4ab75 29 cmd[1]=0x04;//75 samples/sec
soonerbot 4:adc885f4ab75 30 cmd[2]=0xE0;//largest possible range (8.1 gauss)
soonerbot 4:adc885f4ab75 31 cmd[3]=0x00;//continuous mode
soonerbot 1:c28fac16a109 32 //pc.printf("%d\r\n",accel.write(magaddr,cmd,2));
soonerbot 4:adc885f4ab75 33 DBGPRINT("%d\r\n",accel.write(magaddr,cmd,4));
soonerbot 1:c28fac16a109 34 return 1;
soonerbot 1:c28fac16a109 35 }
soonerbot 1:c28fac16a109 36
soonerbot 4:adc885f4ab75 37 float gyroReader::checkCompass(){
soonerbot 4:adc885f4ab75 38 char com[1];
soonerbot 4:adc885f4ab75 39 char cmd[6]={0,0,0,0,0,0};
soonerbot 4:adc885f4ab75 40 char addr=0x3C;//0b0011110;
soonerbot 4:adc885f4ab75 41 com[0]=0x03|0x80;
soonerbot 4:adc885f4ab75 42 accel.write(addr, com, 1,true);
soonerbot 4:adc885f4ab75 43 accel.read(addr, cmd, 6);
soonerbot 4:adc885f4ab75 44 xmag=(signed short)((((unsigned char)cmd[1])<<8)+(unsigned char)cmd[0]);
soonerbot 4:adc885f4ab75 45 ymag=(signed short)((((unsigned char)cmd[3])<<8)+(unsigned char)cmd[2]);
soonerbot 4:adc885f4ab75 46 zmag=(signed short)((((unsigned char)cmd[5])<<8)+(unsigned char)cmd[4]);
soonerbot 4:adc885f4ab75 47 //DBGPRINT("%d\t\t%d\t\t%d\r\n",xmag,ymag,zmag);
soonerbot 4:adc885f4ab75 48 return 0;//atan2((double)xacc,(double)yacc);
soonerbot 4:adc885f4ab75 49 }
soonerbot 4:adc885f4ab75 50
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 1:c28fac16a109 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 1:c28fac16a109 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 1:c28fac16a109 95 int gyroReader::checkGyro(int& xacc, int& yacc, int& zacc){
soonerbot 1:c28fac16a109 96 char com[1];
soonerbot 1:c28fac16a109 97 char cmd[6]={0,0,0,0,0,0};
soonerbot 3:a223b0bf8256 98 char addr=0xD6;//0b0011000;
soonerbot 1:c28fac16a109 99 com[0]=0x28|0x80;
soonerbot 1:c28fac16a109 100 accel.write(addr, com, 1,true);
soonerbot 1:c28fac16a109 101 accel.read(addr, cmd, 6);
soonerbot 1:c28fac16a109 102 //int x=cmd
soonerbot 1:c28fac16a109 103 /*accel.start();
soonerbot 1:c28fac16a109 104 accel.write(addr);
soonerbot 1:c28fac16a109 105 accel.write(0x28|0x80);
soonerbot 1:c28fac16a109 106 accel.start();
soonerbot 1:c28fac16a109 107 accel.write(addr|1);
soonerbot 1:c28fac16a109 108 cmd[0]=accel.read(1);
soonerbot 1:c28fac16a109 109 cmd[1]=accel.read(1);
soonerbot 1:c28fac16a109 110 cmd[2]=accel.read(1);
soonerbot 1:c28fac16a109 111 cmd[3]=accel.read(0);
soonerbot 1:c28fac16a109 112 accel.stop();*/
soonerbot 1:c28fac16a109 113 //int xacc=((int)cmd[3]<<8)|cmd[2];
soonerbot 1:c28fac16a109 114 //int xacc=((int)cmd[1]<<8)|cmd[0];
soonerbot 1:c28fac16a109 115 //xacc=-((cmd[1]&0x80)==0)?xacc:-((xacc-1)^0xFFFF);
soonerbot 1:c28fac16a109 116 //xacc=-((cmd[3]&0x80)==0)?xacc:-((xacc-1)^0xFFFF);
soonerbot 1:c28fac16a109 117 //int yacc=(signed char)cmd[3];
soonerbot 1:c28fac16a109 118 //int zacc=(signed char)cmd[5];
soonerbot 1:c28fac16a109 119 xacc=(signed short)((((unsigned char)cmd[1])<<8)+(unsigned char)cmd[0]);
soonerbot 1:c28fac16a109 120 yacc=(signed short)((((unsigned char)cmd[3])<<8)+(unsigned char)cmd[2]);
soonerbot 1:c28fac16a109 121 zacc=(signed short)((((unsigned char)cmd[5])<<8)+(unsigned char)cmd[4]);
soonerbot 1:c28fac16a109 122 //float angle=atan(zacc==0?yacc*1000000.0:(float)yacc/(float)zacc)-(zacc>=0?0.1:3.141592);
soonerbot 1:c28fac16a109 123 //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 124 return xacc;
soonerbot 1:c28fac16a109 125 }
soonerbot 1:c28fac16a109 126
soonerbot 1:c28fac16a109 127 gyroReader::gyroReader(PinName pinA, PinName pinB) : accel(pinA,pinB)/*, upkeepTimer(gyroReader::gyroUpkeep, osTimerPeriodic, (void *)0), testPin(test)*/{
soonerbot 1:c28fac16a109 128 gyroZcount=0;
soonerbot 1:c28fac16a109 129 gyroXcount=0;
soonerbot 3:a223b0bf8256 130 zcal=-54;
soonerbot 3:a223b0bf8256 131 xcal=120;
soonerbot 1:c28fac16a109 132 polls=0;
soonerbot 1:c28fac16a109 133 startAccel();
soonerbot 4:adc885f4ab75 134 xmag=ymag=zmag=0;
soonerbot 1:c28fac16a109 135 //testPin=0;
soonerbot 1:c28fac16a109 136 //upkeepTimer.start(10);
soonerbot 1:c28fac16a109 137 gyroUpkeepTicker.attach_us(this,&gyroReader::gyroUpkeep,10000);
soonerbot 1:c28fac16a109 138 }
soonerbot 1:c28fac16a109 139 void gyroReader::setLevel(){
soonerbot 1:c28fac16a109 140 gyroXcount=0;
soonerbot 1:c28fac16a109 141 }
soonerbot 1:c28fac16a109 142
soonerbot 1:c28fac16a109 143 void gyroReader::resetZ(){
soonerbot 1:c28fac16a109 144 gyroZcount=0;
soonerbot 1:c28fac16a109 145 }
soonerbot 1:c28fac16a109 146
soonerbot 1:c28fac16a109 147 int gyroReader::getZ(){
soonerbot 1:c28fac16a109 148 return gyroZcount;
soonerbot 1:c28fac16a109 149 }
soonerbot 1:c28fac16a109 150 int gyroReader::getX(){
soonerbot 1:c28fac16a109 151 return gyroXcount;
soonerbot 1:c28fac16a109 152 }
soonerbot 1:c28fac16a109 153 void gyroReader::reset(){
soonerbot 1:c28fac16a109 154 gyroZcount=gyroXcount=polls=0;
soonerbot 1:c28fac16a109 155 }
soonerbot 1:c28fac16a109 156
soonerbot 1:c28fac16a109 157 void gyroReader::calibrate(){
soonerbot 1:c28fac16a109 158 zcal=-(gyroZcount/polls)+zcal;
soonerbot 1:c28fac16a109 159 xcal=-(gyroXcount/polls)+xcal;
soonerbot 3:a223b0bf8256 160 DBGPRINT("cal: %d, %d (%d)\r\n",zcal,xcal,polls);
soonerbot 4:adc885f4ab75 161 }
soonerbot 4:adc885f4ab75 162
soonerbot 4:adc885f4ab75 163 int gyroReader::compZ(int compval){
soonerbot 4:adc885f4ab75 164 int midval= (compval-gyroZcount)%4050000;
soonerbot 4:adc885f4ab75 165 if(midval > 2025000){
soonerbot 4:adc885f4ab75 166 midval-=4050000;
soonerbot 4:adc885f4ab75 167 }
soonerbot 4:adc885f4ab75 168 return midval;
soonerbot 4:adc885f4ab75 169 }
soonerbot 4:adc885f4ab75 170
soonerbot 4:adc885f4ab75 171 double gyroReader::getZDegrees(){
soonerbot 4:adc885f4ab75 172 return gyroZcount*360.0/4050000.0;
soonerbot 4:adc885f4ab75 173 }