Kobayashi Akihiro / ActiveCaster

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.

Dependents:   ActiveCaster_ ActiveCaster_2

AMT22.cpp@2:f206311600ee, 2022-01-28 (annotated)

Committer:
e5119053f6
Date:
Fri Jan 28 15:43:18 2022 +0000
Revision:
2:f206311600ee
Parent:
0:5e4f1e288e2a 
DDSS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e5119053f6 0:5e4f1e288e2a 1 #include "AMT22.h"
e5119053f6 0:5e4f1e288e2a 2
e5119053f6 0:5e4f1e288e2a 3 AMT203V::AMT203V(SPI* xSPI, PinName xCSBpin){
e5119053f6 0:5e4f1e288e2a 4 CSBpin = new DigitalOut(xCSBpin);
e5119053f6 0:5e4f1e288e2a 5 pSPI = xSPI;
e5119053f6 0:5e4f1e288e2a 6 init_done = false;
e5119053f6 0:5e4f1e288e2a 7 }
e5119053f6 0:5e4f1e288e2a 8
e5119053f6 0:5e4f1e288e2a 9 // SPI送信部分
e5119053f6 0:5e4f1e288e2a 10 int AMT203V::spi_write(int msg){
e5119053f6 0:5e4f1e288e2a 11 int msg_temp = 0;
e5119053f6 0:5e4f1e288e2a 12 CSBpin->write(0);
e5119053f6 0:5e4f1e288e2a 13 wait_us(3);
e5119053f6 0:5e4f1e288e2a 14 msg_temp = pSPI->write(msg);
e5119053f6 0:5e4f1e288e2a 15 CSBpin->write(1);
e5119053f6 0:5e4f1e288e2a 16 wait_us(3);
e5119053f6 0:5e4f1e288e2a 17 return(msg_temp);
e5119053f6 0:5e4f1e288e2a 18 }
e5119053f6 0:5e4f1e288e2a 19
e5119053f6 0:5e4f1e288e2a 20 int AMT203V::init(){
e5119053f6 0:5e4f1e288e2a 21
e5119053f6 0:5e4f1e288e2a 22 CSBpin->write(1);
e5119053f6 0:5e4f1e288e2a 23 pSPI->frequency(2000000);
e5119053f6 0:5e4f1e288e2a 24
e5119053f6 0:5e4f1e288e2a 25 int ret = getRawEncount();
e5119053f6 0:5e4f1e288e2a 26 preABSposition = ABSposition;
e5119053f6 0:5e4f1e288e2a 27
e5119053f6 0:5e4f1e288e2a 28 if(ret == -1) return -1;
e5119053f6 0:5e4f1e288e2a 29 else init_done = true;
e5119053f6 0:5e4f1e288e2a 30
e5119053f6 0:5e4f1e288e2a 31 return 1;
e5119053f6 0:5e4f1e288e2a 32 }
e5119053f6 0:5e4f1e288e2a 33
e5119053f6 0:5e4f1e288e2a 34 int AMT203V::getRawEncount(){
e5119053f6 0:5e4f1e288e2a 35 int recieved;
e5119053f6 0:5e4f1e288e2a 36 int recieve_count = 0;
e5119053f6 0:5e4f1e288e2a 37 int error_count = 0;
e5119053f6 0:5e4f1e288e2a 38 bool recieve_done = false;
e5119053f6 0:5e4f1e288e2a 39
e5119053f6 0:5e4f1e288e2a 40 unsigned int rawValue = 0;
e5119053f6 0:5e4f1e288e2a 41 bool correct_answer = false;
e5119053f6 0:5e4f1e288e2a 42
e5119053f6 0:5e4f1e288e2a 43 CSBpin->write(0);
e5119053f6 0:5e4f1e288e2a 44 wait_us(3);
e5119053f6 0:5e4f1e288e2a 45 rawValue |= pSPI->write(0x00) << 8;
e5119053f6 0:5e4f1e288e2a 46 wait_us(3);
e5119053f6 0:5e4f1e288e2a 47 rawValue |= pSPI->write(0x00);
e5119053f6 0:5e4f1e288e2a 48 wait_us(3);
e5119053f6 0:5e4f1e288e2a 49 CSBpin->write(1);
e5119053f6 0:5e4f1e288e2a 50 wait_us(3);
e5119053f6 0:5e4f1e288e2a 51
e5119053f6 0:5e4f1e288e2a 52 bool odd, even;
e5119053f6 0:5e4f1e288e2a 53 for(int i = 0; i < 14; i++){
e5119053f6 0:5e4f1e288e2a 54 if(i % 2){
e5119053f6 0:5e4f1e288e2a 55 odd ^= (rawValue >> i) & 0x01;
e5119053f6 0:5e4f1e288e2a 56 }else{
e5119053f6 0:5e4f1e288e2a 57 even ^= (rawValue >> i) & 0x01;
e5119053f6 0:5e4f1e288e2a 58 }
e5119053f6 0:5e4f1e288e2a 59 }
e5119053f6 0:5e4f1e288e2a 60 odd = !odd;
e5119053f6 0:5e4f1e288e2a 61 even = !even;
e5119053f6 0:5e4f1e288e2a 62
e5119053f6 0:5e4f1e288e2a 63 correct_answer = (((rawValue >> 15) & 0x01) == odd) && (((rawValue >> 14) & 0x01) == even);
e5119053f6 0:5e4f1e288e2a 64
e5119053f6 0:5e4f1e288e2a 65 if(correct_answer){
e5119053f6 0:5e4f1e288e2a 66 ABSposition = rawValue & 0x3FFF;
e5119053f6 0:5e4f1e288e2a 67 //printf("ret : %d %x ", ABSposition, rawValue);
e5119053f6 0:5e4f1e288e2a 68 }/*else{
e5119053f6 0:5e4f1e288e2a 69 printf("xxx : %d %x\n", rawValue, rawValue);
e5119053f6 0:5e4f1e288e2a 70 }*/
e5119053f6 0:5e4f1e288e2a 71
e5119053f6 0:5e4f1e288e2a 72 return 1;
e5119053f6 0:5e4f1e288e2a 73 }
e5119053f6 0:5e4f1e288e2a 74
e5119053f6 0:5e4f1e288e2a 75 int AMT203V::getEncount(){
e5119053f6 0:5e4f1e288e2a 76 if(init_done){
e5119053f6 0:5e4f1e288e2a 77 getRawEncount();
e5119053f6 0:5e4f1e288e2a 78 updateCount();
e5119053f6 0:5e4f1e288e2a 79
e5119053f6 0:5e4f1e288e2a 80 encount = rotation * res + ABSposition;
e5119053f6 0:5e4f1e288e2a 81 preABSposition = ABSposition;
e5119053f6 0:5e4f1e288e2a 82
e5119053f6 0:5e4f1e288e2a 83 //printf(" encount %d abs position %d ", encount, ABSposition);
e5119053f6 0:5e4f1e288e2a 84 }
e5119053f6 0:5e4f1e288e2a 85 else{
e5119053f6 0:5e4f1e288e2a 86 return -1;
e5119053f6 0:5e4f1e288e2a 87 }
e5119053f6 0:5e4f1e288e2a 88 return encount;
e5119053f6 0:5e4f1e288e2a 89 }
e5119053f6 0:5e4f1e288e2a 90
e5119053f6 0:5e4f1e288e2a 91 // 2,1,0の次に4095ではなくマイナスの値になるように，4093,4094,4095の次に0にならず大きな値になるようにしてる関数
e5119053f6 0:5e4f1e288e2a 92 void AMT203V::updateCount(){
e5119053f6 0:5e4f1e288e2a 93 if(abs((int)(preABSposition - ABSposition)) >= (int)(res * 0.75)){//3000){
e5119053f6 0:5e4f1e288e2a 94 if(preABSposition > ABSposition){
e5119053f6 0:5e4f1e288e2a 95 rotation++;
e5119053f6 0:5e4f1e288e2a 96 }else if(preABSposition < ABSposition){
e5119053f6 0:5e4f1e288e2a 97 rotation--;
e5119053f6 0:5e4f1e288e2a 98 }
e5119053f6 0:5e4f1e288e2a 99 }
e5119053f6 0:5e4f1e288e2a 100 }
e5119053f6 0:5e4f1e288e2a 101
e5119053f6 0:5e4f1e288e2a 102 int AMT203V::setZeroPos()
e5119053f6 0:5e4f1e288e2a 103 {
e5119053f6 0:5e4f1e288e2a 104 int response;
e5119053f6 0:5e4f1e288e2a 105 int count = 0;
e5119053f6 0:5e4f1e288e2a 106
e5119053f6 0:5e4f1e288e2a 107 CSBpin->write(0);
e5119053f6 0:5e4f1e288e2a 108 wait_us(3);
e5119053f6 0:5e4f1e288e2a 109 response |= pSPI->write(0x00) << 8;
e5119053f6 0:5e4f1e288e2a 110 wait_us(3);
e5119053f6 0:5e4f1e288e2a 111 response |= pSPI->write(0x70);
e5119053f6 0:5e4f1e288e2a 112 wait_us(3);
e5119053f6 0:5e4f1e288e2a 113 CSBpin->write(1);
e5119053f6 0:5e4f1e288e2a 114
e5119053f6 0:5e4f1e288e2a 115 printf("response %x\n", response);
e5119053f6 0:5e4f1e288e2a 116
e5119053f6 0:5e4f1e288e2a 117 getRawEncount();
e5119053f6 0:5e4f1e288e2a 118 preABSposition = ABSposition;
e5119053f6 0:5e4f1e288e2a 119
e5119053f6 0:5e4f1e288e2a 120 return 1;
e5119053f6 0:5e4f1e288e2a 121 }