AMT22.cpp

00001 #include "AMT22.h"
00002 
00003 AMT203V::AMT203V(SPI* xSPI, PinName xCSBpin){
00004   CSBpin = new DigitalOut(xCSBpin);
00005   pSPI = xSPI;
00006   init_done = false;
00007 }
00008 
00009 // SPI送信部分
00010 int AMT203V::spi_write(int msg){
00011   int msg_temp = 0;
00012   CSBpin->write(0);
00013   wait_us(3);
00014   msg_temp = pSPI->write(msg);
00015   CSBpin->write(1);
00016   wait_us(3);
00017   return(msg_temp);
00018 }
00019 
00020 int AMT203V::init(){
00021 
00022   CSBpin->write(1);
00023   pSPI->frequency(2000000);
00024 
00025   int ret = getRawEncount();
00026   preABSposition = ABSposition;
00027   
00028   if(ret == -1) return -1;
00029   else init_done = true;
00030   
00031   return 1;
00032 }
00033 
00034 int AMT203V::getRawEncount(){
00035   int recieved;
00036   int recieve_count = 0;
00037   int error_count = 0;
00038   bool recieve_done = false;
00039 
00040   unsigned int  rawValue = 0;
00041   bool correct_answer = false;
00042 
00043   CSBpin->write(0);
00044   wait_us(3);
00045   rawValue |= pSPI->write(0x00) << 8;
00046   wait_us(3);
00047   rawValue |= pSPI->write(0x00);
00048   wait_us(3);
00049   CSBpin->write(1);
00050   wait_us(3);
00051     
00052   bool odd, even;
00053   for(int i = 0; i < 14; i++){
00054       if(i % 2){
00055           odd ^= (rawValue >> i) & 0x01;
00056       }else{
00057           even ^= (rawValue >> i) & 0x01;
00058       }
00059   }
00060   odd = !odd;
00061   even = !even;
00062 
00063   correct_answer = (((rawValue >> 15) & 0x01) == odd) && (((rawValue >> 14) & 0x01) == even);
00064 
00065   if(correct_answer){
00066       ABSposition = rawValue & 0x3FFF;
00067       //printf("ret : %d %x ", ABSposition, rawValue);
00068   }/*else{
00069       printf("xxx : %d %x\n", rawValue, rawValue);
00070   }*/
00071   
00072   return 1;
00073 }
00074 
00075 int AMT203V::getEncount(){
00076   if(init_done){
00077     getRawEncount();
00078     updateCount();
00079     
00080     encount = rotation * res + ABSposition;
00081     preABSposition = ABSposition;
00082 
00083     //printf("  encount %d abs position %d       ", encount, ABSposition);
00084   }
00085   else{
00086     return -1;
00087   }
00088   return encount;
00089 }
00090 
00091 // 2,1,0の次に4095ではなくマイナスの値になるように，4093,4094,4095の次に0にならず大きな値になるようにしてる関数
00092 void AMT203V::updateCount(){
00093   if(abs((int)(preABSposition - ABSposition)) >= (int)(res * 0.75)){//3000){
00094     if(preABSposition > ABSposition){
00095       rotation++;
00096     }else if(preABSposition < ABSposition){
00097       rotation--;
00098     }
00099   }
00100 }
00101 
00102 int AMT203V::setZeroPos()
00103 {
00104   int response;
00105   int count = 0;
00106 
00107   CSBpin->write(0);
00108   wait_us(3);
00109   response |= pSPI->write(0x00) << 8;
00110   wait_us(3);
00111   response |= pSPI->write(0x70);
00112   wait_us(3);
00113   CSBpin->write(1);
00114 
00115   printf("response %x\n", response);
00116 
00117   getRawEncount();
00118   preABSposition = ABSposition;
00119 
00120   return 1;
00121 }