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

remoteEnc.cpp@1:c28fac16a109, 2013-11-17 (annotated)

Committer:
soonerbot
Date:
Sun Nov 17 03:05:35 2013 +0000
Revision:
1:c28fac16a109
Child:
7:3b2cf7efe5d1
Added encoder direction settings and setup motors and encoders

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soonerbot 1:c28fac16a109 1 #include "remoteEnc.h"
soonerbot 1:c28fac16a109 2 #include "dbgprint.h"
soonerbot 1:c28fac16a109 3
soonerbot 1:c28fac16a109 4
soonerbot 1:c28fac16a109 5 /*private:
soonerbot 1:c28fac16a109 6 SPI &spi;
soonerbot 1:c28fac16a109 7 DigitalIn cs;
soonerbot 1:c28fac16a109 8 int referenceCounts[4];
soonerbot 1:c28fac16a109 9 int latestCount[4];
soonerbot 1:c28fac16a109 10 int outState;
soonerbot 1:c28fac16a109 11 public:
soonerbot 1:c28fac16a109 12 remoteEnc(SPI &spiIn, PinName cspin);
soonerbot 1:c28fac16a109 13 const int* getVals();
soonerbot 1:c28fac16a109 14 int setPins(int mask);
soonerbot 1:c28fac16a109 15 int setPin(int pin, int state);
soonerbot 1:c28fac16a109 16 int getPinState();
soonerbot 1:c28fac16a109 17 int reset(int slot);
soonerbot 1:c28fac16a109 18 int getLastVal(int slot);*/
soonerbot 1:c28fac16a109 19
soonerbot 1:c28fac16a109 20 remoteEnc::remoteEnc(SPI &spiIn, PinName cspin) : spi(spiIn), cs(cspin){
soonerbot 1:c28fac16a109 21 cs=1;
soonerbot 1:c28fac16a109 22 spi.format(8,0);
soonerbot 1:c28fac16a109 23 spi.frequency(100000);
soonerbot 1:c28fac16a109 24 direction[0]=direction[1]=direction[2]=direction[3]=1;
soonerbot 1:c28fac16a109 25 int i;
soonerbot 1:c28fac16a109 26 for(i=0;i<4;i++){
soonerbot 1:c28fac16a109 27 referenceCounts[i]=0;
soonerbot 1:c28fac16a109 28 latestQuadrant[i]=0;
soonerbot 1:c28fac16a109 29 }
soonerbot 1:c28fac16a109 30 getVals();
soonerbot 1:c28fac16a109 31 for(i=0;i<4;i++){
soonerbot 1:c28fac16a109 32 latestQuadrant[i]=latestActual[i]&(~0x3FFF);
soonerbot 1:c28fac16a109 33 DBGPRINT("{%x,%x}",latestQuadrant[i],latestActual[i]);
soonerbot 1:c28fac16a109 34 }
soonerbot 1:c28fac16a109 35 getVals();
soonerbot 1:c28fac16a109 36 for(i=0;i<4;i++){
soonerbot 1:c28fac16a109 37 referenceCounts[i]=latestCount[i];
soonerbot 1:c28fac16a109 38 DBGPRINT("-]%x,%x[-",latestQuadrant[i],latestActual[i]);
soonerbot 1:c28fac16a109 39 }
soonerbot 1:c28fac16a109 40 //DBGPRINT("reset to %d, %d, %d, %d\r\n",referenceCounts[0],referenceCounts[1],referenceCounts[2],referenceCounts[3]);
soonerbot 1:c28fac16a109 41
soonerbot 1:c28fac16a109 42 //setPins(0);
soonerbot 1:c28fac16a109 43 outState=0;
soonerbot 1:c28fac16a109 44 }
soonerbot 1:c28fac16a109 45
soonerbot 1:c28fac16a109 46 const int* remoteEnc::getVals(){
soonerbot 1:c28fac16a109 47 int charBuf[8];
soonerbot 1:c28fac16a109 48 cs=0;
soonerbot 1:c28fac16a109 49 spi.write(0);
soonerbot 1:c28fac16a109 50 spi.write(1);
soonerbot 1:c28fac16a109 51 int i;
soonerbot 1:c28fac16a109 52 for(i=2;i<=9;i++)
soonerbot 1:c28fac16a109 53 charBuf[i-2]=spi.write(i);
soonerbot 1:c28fac16a109 54 cs=1;
soonerbot 1:c28fac16a109 55 for(i=0;i<4;i++){
soonerbot 1:c28fac16a109 56 latestActual[i]=(charBuf[2*i]+(charBuf[2*i+1]<<8))*direction[i];
soonerbot 1:c28fac16a109 57 //DBGPRINT("[%d, %d]",i,latestActual[i]);
soonerbot 1:c28fac16a109 58 if((latestActual[i]&0xC000)!=(latestQuadrant[i]&0xC000)){
soonerbot 1:c28fac16a109 59 DBGPRINT("*%x and %x to ",latestActual[i],latestQuadrant[i]);
soonerbot 1:c28fac16a109 60 if((latestActual[i]&0xC000)==((latestQuadrant[i]+0x4000)&0xC000)){
soonerbot 1:c28fac16a109 61 latestQuadrant[i]+=0x4000;
soonerbot 1:c28fac16a109 62 } else if ((latestActual[i]&0xC000)==((latestQuadrant[i]-0x4000)&0xC000)){
soonerbot 1:c28fac16a109 63 latestQuadrant[i]-=0x4000;
soonerbot 1:c28fac16a109 64 }
soonerbot 1:c28fac16a109 65 DBGPRINT("%x and %x\r\n",latestActual[i],latestQuadrant[i]);
soonerbot 1:c28fac16a109 66 }
soonerbot 1:c28fac16a109 67 //latestCount[i]=charBuf[2*i]+(charBuf[2*i+1]<<8)-referenceCounts[i];
soonerbot 1:c28fac16a109 68 latestCount[i]=latestQuadrant[i]+(latestActual[i]&0x3FFF)-referenceCounts[i];
soonerbot 1:c28fac16a109 69 }
soonerbot 1:c28fac16a109 70 return latestCount;
soonerbot 1:c28fac16a109 71 }
soonerbot 1:c28fac16a109 72
soonerbot 1:c28fac16a109 73 int remoteEnc::getLastVal(int slot){
soonerbot 1:c28fac16a109 74 return latestCount[slot];
soonerbot 1:c28fac16a109 75 }
soonerbot 1:c28fac16a109 76
soonerbot 1:c28fac16a109 77 int remoteEnc::getSingleValue(int slot){
soonerbot 1:c28fac16a109 78 cs=0;
soonerbot 1:c28fac16a109 79 spi.write(slot*2);
soonerbot 1:c28fac16a109 80 spi.write(slot*2+1);
soonerbot 1:c28fac16a109 81 int lowbit=spi.write(0);
soonerbot 1:c28fac16a109 82 int highbit=spi.write(0);
soonerbot 1:c28fac16a109 83 cs=1;
soonerbot 1:c28fac16a109 84 latestActual[slot]=(lowbit+(highbit<<8))*direction[slot];
soonerbot 1:c28fac16a109 85 //DBGPRINT("[%d, %d]",slot,latestActual[slot]);
soonerbot 1:c28fac16a109 86 if((latestActual[slot]&0xC000)!=(latestQuadrant[slot]&0xC000)){
soonerbot 1:c28fac16a109 87 DBGPRINT("*%x and %x to ",latestActual[slot],latestQuadrant[slot]);
soonerbot 1:c28fac16a109 88 if((latestActual[slot]&0xC000)==((latestQuadrant[slot]+0x4000)&0xC000)){
soonerbot 1:c28fac16a109 89 latestQuadrant[slot]+=0x4000;
soonerbot 1:c28fac16a109 90 } else if ((latestActual[slot]&0xC000)==((latestQuadrant[slot]-0x4000)&0xC000)){
soonerbot 1:c28fac16a109 91 latestQuadrant[slot]-=0x4000;
soonerbot 1:c28fac16a109 92 }
soonerbot 1:c28fac16a109 93 DBGPRINT("%x and %x\r\n",latestActual[slot],latestQuadrant[slot]);
soonerbot 1:c28fac16a109 94 }
soonerbot 1:c28fac16a109 95 //latestCount[slot]=charBuf[2*slot]+(charBuf[2*slot+1]<<8)-referenceCounts[slot];
soonerbot 1:c28fac16a109 96 latestCount[slot]=latestQuadrant[slot]+(latestActual[slot]&0x3FFF)-referenceCounts[slot];
soonerbot 1:c28fac16a109 97 return latestCount[slot];
soonerbot 1:c28fac16a109 98 }
soonerbot 1:c28fac16a109 99
soonerbot 1:c28fac16a109 100 int remoteEnc::setDirections(int dir1, int dir2, int dir3, int dir4){
soonerbot 1:c28fac16a109 101 direction[0]= (dir1 == -1)? -1: 1;
soonerbot 1:c28fac16a109 102 direction[1]= (dir2 == -1)? -1: 1;
soonerbot 1:c28fac16a109 103 direction[2]= (dir3 == -1)? -1: 1;
soonerbot 1:c28fac16a109 104 direction[3]= (dir4 == -1)? -1: 1;
soonerbot 1:c28fac16a109 105 int i;
soonerbot 1:c28fac16a109 106 for(i=0;i<4;i++){
soonerbot 1:c28fac16a109 107 referenceCounts[i]=0;
soonerbot 1:c28fac16a109 108 latestQuadrant[i]=0;
soonerbot 1:c28fac16a109 109 }
soonerbot 1:c28fac16a109 110 getVals();
soonerbot 1:c28fac16a109 111 for(i=0;i<4;i++){
soonerbot 1:c28fac16a109 112 latestQuadrant[i]=latestActual[i]&(~0x3FFF);
soonerbot 1:c28fac16a109 113 //DBGPRINT("{%x,%x}",latestQuadrant[i],latestActual[i]);
soonerbot 1:c28fac16a109 114 }
soonerbot 1:c28fac16a109 115 getVals();
soonerbot 1:c28fac16a109 116 for(i=0;i<4;i++){
soonerbot 1:c28fac16a109 117 referenceCounts[i]=latestCount[i];
soonerbot 1:c28fac16a109 118 //DBGPRINT("{{%x,%x}}",latestQuadrant[i],latestActual[i]);
soonerbot 1:c28fac16a109 119 }
soonerbot 1:c28fac16a109 120 return 1;
soonerbot 1:c28fac16a109 121 }
soonerbot 1:c28fac16a109 122
soonerbot 1:c28fac16a109 123 int remoteEnc::reset(int slot){
soonerbot 1:c28fac16a109 124 referenceCounts[slot] += getSingleValue(slot);
soonerbot 1:c28fac16a109 125 return 1;
soonerbot 1:c28fac16a109 126 }
soonerbot 1:c28fac16a109 127
soonerbot 1:c28fac16a109 128 int remoteEnc::resetAll(){
soonerbot 1:c28fac16a109 129 const int *buf = getVals();
soonerbot 1:c28fac16a109 130 int i;
soonerbot 1:c28fac16a109 131 for(i=0;i<4;i++)
soonerbot 1:c28fac16a109 132 referenceCounts[i] += buf[i];
soonerbot 1:c28fac16a109 133 return 1;
soonerbot 1:c28fac16a109 134 }
soonerbot 1:c28fac16a109 135
soonerbot 1:c28fac16a109 136
soonerbot 1:c28fac16a109 137 int remoteEnc::setPins(int mask){
soonerbot 1:c28fac16a109 138 cs=0;
soonerbot 1:c28fac16a109 139 spi.write((mask&0x3F)|0x80);
soonerbot 1:c28fac16a109 140 spi.write(0x01);
soonerbot 1:c28fac16a109 141 //DBGPRINT("%X,%X: %d\r\n",mask,(mask&0x3F)|0x80, spi.write(0x01));
soonerbot 1:c28fac16a109 142 //DBGPRINT("%X,%X: %d\r\n",mask,(mask&0x3F)|0x80, spi.write(0x01));
soonerbot 1:c28fac16a109 143 //DBGPRINT("%X,%X: %d\r\n",mask,(mask&0x3F)|0x80, spi.write(0x01));
soonerbot 1:c28fac16a109 144 cs=1;
soonerbot 1:c28fac16a109 145 outState=mask;
soonerbot 1:c28fac16a109 146 return 1;
soonerbot 1:c28fac16a109 147 }
soonerbot 1:c28fac16a109 148
soonerbot 1:c28fac16a109 149
soonerbot 1:c28fac16a109 150 int remoteEnc::setPin(int pin, int state){
soonerbot 1:c28fac16a109 151 if(state==0){
soonerbot 1:c28fac16a109 152 outState &=~ (1<<pin);
soonerbot 1:c28fac16a109 153 } else {
soonerbot 1:c28fac16a109 154 outState |= (1<<pin);
soonerbot 1:c28fac16a109 155 }
soonerbot 1:c28fac16a109 156 setPins(outState);
soonerbot 1:c28fac16a109 157 return 1;
soonerbot 1:c28fac16a109 158 }
soonerbot 1:c28fac16a109 159