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@25:c21d35c7f0de, 2017-07-01 (annotated)

Committer:
sift
Date:
Sat Jul 01 00:26:28 2017 +0000
Revision:
25:c21d35c7f0de
Parent:
24:1de0291bc5eb
Child:
28:47e9531a3a9d
??????????????????; (????????? ???)

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 21:bbf2ad7e6602 7 #define ratioLPF 0.0609f //cut off:10Hz(at 1ms sampling)
sift 2:9d69f27a3d3b 8
sift 0:276c1dab2d62 9 extern SPI spi;
sift 24:1de0291bc5eb 10 extern DigitalOut ioExpCs;
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 24:1de0291bc5eb 22 ioExpCs = 1;
sift 0:276c1dab2d62 23 wait(0.1); //Omajinai
sift 0:276c1dab2d62 24
sift 24:1de0291bc5eb 25 ioExpCs = 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 24:1de0291bc5eb 29 ioExpCs = 1;
sift 0:276c1dab2d62 30 wait_us(10); //Omajinai
sift 0:276c1dab2d62 31
sift 24:1de0291bc5eb 32 ioExpCs = 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 24:1de0291bc5eb 36 ioExpCs = 1;
sift 0:276c1dab2d62 37 wait_us(10);
sift 0:276c1dab2d62 38
sift 24:1de0291bc5eb 39 ioExpCs = 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 24:1de0291bc5eb 43 ioExpCs = 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 24:1de0291bc5eb 51 ioExpCs = 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 24:1de0291bc5eb 55 ioExpCs = 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 if(steerIntFlag == true) {
sift 2:9d69f27a3d3b 70 steerIntFlag = false;
sift 2:9d69f27a3d3b 71 int readData = GTB[readIoExp()];
sift 2:9d69f27a3d3b 72
sift 2:9d69f27a3d3b 73 if(readData == 127) {
sift 20:3c5061281a7a 74 LED[2] = LED[3] = 1;
sift 2:9d69f27a3d3b 75 } else if(readData < 127) {
sift 20:3c5061281a7a 76 LED[2] = 1;
sift 20:3c5061281a7a 77 LED[3] = 0;
sift 20:3c5061281a7a 78 } else {
sift 2:9d69f27a3d3b 79 LED[2] = 0;
sift 20:3c5061281a7a 80 LED[3] = 1;
sift 2:9d69f27a3d3b 81 }
sift 2:9d69f27a3d3b 82
sift 13:6dc51981f391 83 //gSteerAngle = (int)(readData*ratioLPF + preAngle*(1.0f-ratioLPF));
sift 13:6dc51981f391 84 gSteerAngle = readData;
sift 2:9d69f27a3d3b 85 }
sift 2:9d69f27a3d3b 86 }
sift 2:9d69f27a3d3b 87
sift 2:9d69f27a3d3b 88 float getSteerAngle(void)
sift 2:9d69f27a3d3b 89 {
sift 2:9d69f27a3d3b 90 //gSteerAngle:0~255
sift 2:9d69f27a3d3b 91 //rad:0~2PI
sift 2:9d69f27a3d3b 92 //return:-PI~PI
sift 2:9d69f27a3d3b 93 float rad = 2.0f*M_PI*(gSteerAngle/255.0f);
sift 2:9d69f27a3d3b 94
sift 2:9d69f27a3d3b 95 if(rad > 2*M_PI)
sift 2:9d69f27a3d3b 96 rad = 2*M_PI;
sift 2:9d69f27a3d3b 97 else if(rad < 0)
sift 2:9d69f27a3d3b 98 rad = 0;
sift 2:9d69f27a3d3b 99
sift 22:95c1f753ecad 100 return rad - M_PI; //steer left->[+]
sift 2:9d69f27a3d3b 101 }
sift 2:9d69f27a3d3b 102
sift 2:9d69f27a3d3b 103 int getSteerDirection(void)
sift 2:9d69f27a3d3b 104 {
sift 14:7cc98e159c6e 105 return ((gSteerAngle > 127) ? 1 : 0); //steer left->1
sift 2:9d69f27a3d3b 106 }
sift 2:9d69f27a3d3b 107
sift 0:276c1dab2d62 108 void initSteering(void)
sift 0:276c1dab2d62 109 {
sift 2:9d69f27a3d3b 110 initIoExp();
sift 21:bbf2ad7e6602 111 ticker.attach(loadSteerAngleISR, 0.001);
sift 2:9d69f27a3d3b 112 }