2014 sift / Mbed 2 deprecated TVDctrller2017_brdRev1_PandA

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 TVDctrller2017_brdRev1_ver6 by 2014 sift

Steering.cpp@20:3c5061281a7a, 2016-08-27 (annotated)

Committer:
sift
Date:
Sat Aug 27 22:08:24 2016 +0000
Revision:
20:3c5061281a7a
Parent:
14:7cc98e159c6e
Child:
21:bbf2ad7e6602
??????????; ????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sift 0:276c1dab2d62 1 #include "mbed.h"
sift 0:276c1dab2d62 2 #include "Steering.h"
sift 0:276c1dab2d62 3
sift 2:9d69f27a3d3b 4 #define myAbs(x) ((x>0)?(x):(-(x)))
sift 2:9d69f27a3d3b 5
sift 2:9d69f27a3d3b 6 #define M_PI 3.1415f
sift 2:9d69f27a3d3b 7 #define ratioLPF 0.4559f //cut off:10Hz(at 10ms sampling)
sift 2:9d69f27a3d3b 8
sift 0:276c1dab2d62 9 extern SPI spi;
sift 0:276c1dab2d62 10 extern DigitalOut cs;
sift 2:9d69f27a3d3b 11 extern DigitalOut LED[];
sift 2:9d69f27a3d3b 12
sift 2:9d69f27a3d3b 13 Ticker ticker;
sift 2:9d69f27a3d3b 14
sift 2:9d69f27a3d3b 15 int gSteerAngle = 0;
sift 0:276c1dab2d62 16
sift 0:276c1dab2d62 17 void initIoExp(void)
sift 0:276c1dab2d62 18 {
sift 0:276c1dab2d62 19 //SPIバスに複数のデバイスをぶら下げる場合，modeをそろえる必要がある
sift 0:276c1dab2d62 20 spi.format(8, 0);
sift 0:276c1dab2d62 21 spi.frequency(1000000);
sift 0:276c1dab2d62 22 cs = 1;
sift 0:276c1dab2d62 23 wait(0.1); //Omajinai
sift 0:276c1dab2d62 24
sift 0:276c1dab2d62 25 cs = 0;
sift 0:276c1dab2d62 26 spi.write(IOEXP_WRITEADD);
sift 0:276c1dab2d62 27 spi.write(IOEXP_IODIR); //IO方向レジスタ
sift 0:276c1dab2d62 28 spi.write(0xFF); //All input
sift 0:276c1dab2d62 29 cs = 1;
sift 0:276c1dab2d62 30 wait_us(10); //Omajinai
sift 0:276c1dab2d62 31
sift 0:276c1dab2d62 32 cs = 0;
sift 0:276c1dab2d62 33 spi.write(IOEXP_WRITEADD);
sift 0:276c1dab2d62 34 spi.write(IOEXP_IPOL); //入力極性
sift 0:276c1dab2d62 35 spi.write(0xFF); //All inverted
sift 0:276c1dab2d62 36 cs = 1;
sift 0:276c1dab2d62 37 wait_us(10);
sift 0:276c1dab2d62 38
sift 0:276c1dab2d62 39 cs = 0;
sift 0:276c1dab2d62 40 spi.write(IOEXP_WRITEADD);
sift 0:276c1dab2d62 41 spi.write(IOEXP_GPPU); //PULL-UP
sift 0:276c1dab2d62 42 spi.write(0xFF); //ALL PULL-UP
sift 0:276c1dab2d62 43 cs = 1;
sift 0:276c1dab2d62 44 wait_us(10);
sift 0:276c1dab2d62 45 }
sift 0:276c1dab2d62 46
sift 0:276c1dab2d62 47 int readIoExp(void)
sift 0:276c1dab2d62 48 {
sift 0:276c1dab2d62 49 int gpio = 0;
sift 0:276c1dab2d62 50
sift 0:276c1dab2d62 51 cs = 0;
sift 0:276c1dab2d62 52 spi.write(IOEXP_READADD);
sift 0:276c1dab2d62 53 spi.write(IOEXP_GPIO);
sift 0:276c1dab2d62 54 gpio = spi.write(0x00);
sift 0:276c1dab2d62 55 cs = 1;
sift 0:276c1dab2d62 56 //wait_us(10);
sift 0:276c1dab2d62 57 return gpio;
sift 0:276c1dab2d62 58 }
sift 0:276c1dab2d62 59
sift 2:9d69f27a3d3b 60 volatile bool steerIntFlag = false;
sift 2:9d69f27a3d3b 61
sift 2:9d69f27a3d3b 62 void loadSteerAngleISR(void)
sift 2:9d69f27a3d3b 63 {
sift 2:9d69f27a3d3b 64 steerIntFlag = true;
sift 2:9d69f27a3d3b 65 }
sift 2:9d69f27a3d3b 66
sift 2:9d69f27a3d3b 67 void loadSteerAngle(void)
sift 2:9d69f27a3d3b 68 {
sift 2:9d69f27a3d3b 69 static int preAngle=0;
sift 2:9d69f27a3d3b 70
sift 2:9d69f27a3d3b 71 if(steerIntFlag == true) {
sift 2:9d69f27a3d3b 72 steerIntFlag = false;
sift 2:9d69f27a3d3b 73
sift 2:9d69f27a3d3b 74 int readData = GTB[readIoExp()];
sift 2:9d69f27a3d3b 75
sift 2:9d69f27a3d3b 76 if(readData == 127) {
sift 20:3c5061281a7a 77 LED[2] = LED[3] = 1;
sift 2:9d69f27a3d3b 78 } else if(readData < 127) {
sift 20:3c5061281a7a 79 LED[2] = 1;
sift 20:3c5061281a7a 80 LED[3] = 0;
sift 20:3c5061281a7a 81 } else {
sift 2:9d69f27a3d3b 82 LED[2] = 0;
sift 20:3c5061281a7a 83 LED[3] = 1;
sift 2:9d69f27a3d3b 84 }
sift 2:9d69f27a3d3b 85
sift 13:6dc51981f391 86 //gSteerAngle = (int)(readData*ratioLPF + preAngle*(1.0f-ratioLPF));
sift 13:6dc51981f391 87 gSteerAngle = readData;
sift 2:9d69f27a3d3b 88
sift 2:9d69f27a3d3b 89 preAngle = gSteerAngle;
sift 2:9d69f27a3d3b 90 }
sift 2:9d69f27a3d3b 91 }
sift 2:9d69f27a3d3b 92
sift 2:9d69f27a3d3b 93 float getSteerAngle(void)
sift 2:9d69f27a3d3b 94 {
sift 2:9d69f27a3d3b 95 //gSteerAngle:0~255
sift 2:9d69f27a3d3b 96 //rad:0~2PI
sift 2:9d69f27a3d3b 97 //return:-PI~PI
sift 2:9d69f27a3d3b 98 float rad = 2.0f*M_PI*(gSteerAngle/255.0f);
sift 2:9d69f27a3d3b 99
sift 2:9d69f27a3d3b 100 if(rad > 2*M_PI)
sift 2:9d69f27a3d3b 101 rad = 2*M_PI;
sift 2:9d69f27a3d3b 102 else if(rad < 0)
sift 2:9d69f27a3d3b 103 rad = 0;
sift 2:9d69f27a3d3b 104
sift 2:9d69f27a3d3b 105 return myAbs(rad - M_PI); //steer left->[+]
sift 2:9d69f27a3d3b 106 }
sift 2:9d69f27a3d3b 107
sift 2:9d69f27a3d3b 108 int getSteerDirection(void)
sift 2:9d69f27a3d3b 109 {
sift 14:7cc98e159c6e 110 return ((gSteerAngle > 127) ? 1 : 0); //steer left->1
sift 2:9d69f27a3d3b 111 }
sift 2:9d69f27a3d3b 112
sift 0:276c1dab2d62 113 void initSteering(void)
sift 0:276c1dab2d62 114 {
sift 2:9d69f27a3d3b 115 initIoExp();
sift 2:9d69f27a3d3b 116 ticker.attach(loadSteerAngleISR, 0.01);
sift 2:9d69f27a3d3b 117 }