Chen Wei Ting / Mbed 2 deprecated zerotorque_final

zero torque and encoder

Dependencies:   MX28 PID mbed

Fork of LSM9DS1_project_5_zerotorque by Chen Wei Ting

encoder.cpp@5:131450b16ce3, 2018-08-31 (annotated)

Committer:
JJting
Date:
Fri Aug 31 01:41:04 2018 +0000
Revision:
5:131450b16ce3
Parent:
0:c23e915f255b 
????OK P=1500 D=40

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JJting 0:c23e915f255b 1 #include "mbed.h"
JJting 0:c23e915f255b 2 #include "encoder.h"
JJting 0:c23e915f255b 3
JJting 0:c23e915f255b 4 DigitalOut encoder_cs(D9);
JJting 0:c23e915f255b 5 //SPI spi_encoder(D11, D12, D13); // mosi, miso, sclk
JJting 0:c23e915f255b 6 SPI spi_encoder(D4, D5, D3); // mosi, miso, sclk
JJting 0:c23e915f255b 7
JJting 0:c23e915f255b 8 unsigned short encoder_value;
JJting 0:c23e915f255b 9 unsigned short angle = 0;
JJting 0:c23e915f255b 10 unsigned short angle_old;
JJting 0:c23e915f255b 11 int angle_dif;
JJting 0:c23e915f255b 12 int a_dif;
JJting 0:c23e915f255b 13 int Angle = 0;
JJting 0:c23e915f255b 14 unsigned short k = 0;
JJting 0:c23e915f255b 15
JJting 0:c23e915f255b 16 void init_SPI_encoder()
JJting 0:c23e915f255b 17 {
JJting 0:c23e915f255b 18 spi_encoder.format(16,3);
JJting 0:c23e915f255b 19 spi_encoder.frequency(1000000); // 1MHz clock rate
JJting 0:c23e915f255b 20 }
JJting 0:c23e915f255b 21
JJting 0:c23e915f255b 22 void init_encoder()
JJting 0:c23e915f255b 23 {
JJting 0:c23e915f255b 24 encoder_cs = 1; // high：disable the device
JJting 0:c23e915f255b 25 }
JJting 0:c23e915f255b 26
JJting 0:c23e915f255b 27 void angle_measure()
JJting 0:c23e915f255b 28 {
JJting 0:c23e915f255b 29 encoder_cs = 0; // Select the device by seting chip select low
JJting 0:c23e915f255b 30 encoder_value = spi_encoder.write(0x00);
JJting 0:c23e915f255b 31 angle = encoder_value >> 3;
JJting 0:c23e915f255b 32 encoder_cs = 1; // Deselect the device
JJting 0:c23e915f255b 33
JJting 0:c23e915f255b 34 if (k == 0) {
JJting 0:c23e915f255b 35 Angle = 0;
JJting 0:c23e915f255b 36 angle_old = angle;
JJting 0:c23e915f255b 37 k++;
JJting 0:c23e915f255b 38 } else {
JJting 0:c23e915f255b 39 angle_dif = angle_count(angle, angle_old);
JJting 0:c23e915f255b 40 Angle = Angle + angle_dif;
JJting 0:c23e915f255b 41 angle_old = angle;
JJting 0:c23e915f255b 42 }
JJting 0:c23e915f255b 43 }
JJting 0:c23e915f255b 44
JJting 0:c23e915f255b 45 int angle_count(unsigned short now,unsigned short old)
JJting 0:c23e915f255b 46 {
JJting 0:c23e915f255b 47 a_dif = now - old;
JJting 0:c23e915f255b 48 if (a_dif > 4096/2) {
JJting 0:c23e915f255b 49 a_dif = -(4096 - a_dif);
JJting 0:c23e915f255b 50 } else if (a_dif < -4096/2) {
JJting 0:c23e915f255b 51 a_dif = 4096 + a_dif;
JJting 0:c23e915f255b 52 } else {
JJting 0:c23e915f255b 53 a_dif = a_dif;
JJting 0:c23e915f255b 54 }
JJting 0:c23e915f255b 55 return a_dif;
JJting 0:c23e915f255b 56 }