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

TVDcontrolDEMO_main.cpp@0:276c1dab2d62, 2016-07-08 (annotated)

Committer:
sift
Date:
Fri Jul 08 05:29:15 2016 +0000
Revision:
0:276c1dab2d62
Child:
1:4d86ec2fe4b1
?????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sift 0:276c1dab2d62 1 #include "mbed.h"
sift 0:276c1dab2d62 2 #include "TVDCTRL.h"
sift 0:276c1dab2d62 3 #include "APSdefs.h"
sift 0:276c1dab2d62 4 #include "Steering.h"
sift 0:276c1dab2d62 5 #include "MCP4922.h"
sift 0:276c1dab2d62 6
sift 0:276c1dab2d62 7 ////////////////////////////////////////
sift 0:276c1dab2d62 8 //IO宣言
sift 0:276c1dab2d62 9 SPI spi(p5,p6,p7);
sift 0:276c1dab2d62 10 MCP4922 mcp(p5,p7,p8); // MOSI, SCLK, CS
sift 0:276c1dab2d62 11 Serial com2(p9, p10); //xbeeとかデバッグ用
sift 0:276c1dab2d62 12 DigitalOut cs(p11);
sift 0:276c1dab2d62 13 //12
sift 0:276c1dab2d62 14 //13
sift 0:276c1dab2d62 15 //14
sift 0:276c1dab2d62 16 InterruptIn motorPulse1(p15);
sift 0:276c1dab2d62 17 InterruptIn motorPulse2(p16);
sift 0:276c1dab2d62 18
sift 0:276c1dab2d62 19 AnalogIn brake(p17);
sift 0:276c1dab2d62 20 AnalogOut ana(p18);
sift 0:276c1dab2d62 21 AnalogIn apsS(p19); //"S"econdary
sift 0:276c1dab2d62 22 AnalogIn apsP(p20); //"P"rimary
sift 0:276c1dab2d62 23 DigitalOut indicatorLed(p21);
sift 0:276c1dab2d62 24 DigitalOut shutDown(p22);
sift 0:276c1dab2d62 25 DigitalOut MotorPulse[] = {p23, p24};
sift 0:276c1dab2d62 26 DigitalIn RTDSW(p25);
sift 0:276c1dab2d62 27 DigitalIn SDState(p26);
sift 0:276c1dab2d62 28 Serial com1(p28, p27); //インパネとの通信に使用するかも知れなくもないかもしれない
sift 0:276c1dab2d62 29
sift 0:276c1dab2d62 30 DigitalOut LED[] = {LED1, LED2, LED3, LED4};
sift 0:276c1dab2d62 31 //DigitalOut watchDog();
sift 0:276c1dab2d62 32
sift 0:276c1dab2d62 33 #define apsPVol (apsP * 3.3f)
sift 0:276c1dab2d62 34 #define apsSVol (apsS * 3.3f)
sift 0:276c1dab2d62 35
sift 0:276c1dab2d62 36 #define indicateSystem(x) (indicatorLed.write(x))
sift 0:276c1dab2d62 37 #define shutdownSystem(void) (shutDown.write(0))
sift 0:276c1dab2d62 38 #define bootSystem(void) (shutDown.write(1))
sift 0:276c1dab2d62 39 #define isPressedRTD(void) (!RTDSW.read())
sift 0:276c1dab2d62 40 #define isShutdownSystem(void) (SDState.read())
sift 0:276c1dab2d62 41
sift 0:276c1dab2d62 42 int checkSensorPlausibility(void);
sift 0:276c1dab2d62 43 void initIO(void);
sift 0:276c1dab2d62 44
sift 0:276c1dab2d62 45 ////////////////////////////////////////
sift 0:276c1dab2d62 46 //センサーサンプリング関係
sift 0:276c1dab2d62 47 #define ratioLPF 0.034f //CutOff:23.89Hz
sift 0:276c1dab2d62 48 void loadSensorsLPF(void);
sift 0:276c1dab2d62 49 ////////////////////////////////////////
sift 0:276c1dab2d62 50
sift 0:276c1dab2d62 51 /*
sift 0:276c1dab2d62 52 //そのうち実装
sift 0:276c1dab2d62 53 CAN can();
sift 0:276c1dab2d62 54 */
sift 0:276c1dab2d62 55
sift 0:276c1dab2d62 56 float apsPSample, apsSSample, brakeSample;
sift 0:276c1dab2d62 57 float fixVoltage(float vol)
sift 0:276c1dab2d62 58 {
sift 0:276c1dab2d62 59 if(vol > APS_MAX_VOLTAGE)
sift 0:276c1dab2d62 60 return APS_MAX_VOLTAGE;
sift 0:276c1dab2d62 61 else if(APS_MIN_VOLTAGE > vol)
sift 0:276c1dab2d62 62 return APS_MIN_VOLTAGE;
sift 0:276c1dab2d62 63 else
sift 0:276c1dab2d62 64 return vol;
sift 0:276c1dab2d62 65 }
sift 0:276c1dab2d62 66
sift 0:276c1dab2d62 67 //APS信号をモーターコントローラの入力電圧に変換
sift 0:276c1dab2d62 68 //fixVoltageを通してから使うこと
sift 0:276c1dab2d62 69 float fixSpecifiedOutputData(float aps)
sift 0:276c1dab2d62 70 {
sift 0:276c1dab2d62 71 float temp;
sift 0:276c1dab2d62 72
sift 0:276c1dab2d62 73 aps = ((aps<APS_MIN_VOLTAGE) ? 0.0f : (aps-APS_MIN_VOLTAGE)); //オフセット修正
sift 0:276c1dab2d62 74
sift 0:276c1dab2d62 75 /*
sift 0:276c1dab2d62 76 if(aps < APS_DEADBAND)
sift 0:276c1dab2d62 77 temp = 0.0f;
sift 0:276c1dab2d62 78 else {
sift 0:276c1dab2d62 79 aps -= APS_DEADBAND;
sift 0:276c1dab2d62 80 temp = (aps * (DACOUTPUT_VALID_RANGE/(APS_VALID_RANGE)));
sift 0:276c1dab2d62 81 temp *= LIMIT;
sift 0:276c1dab2d62 82 }
sift 0:276c1dab2d62 83
sift 0:276c1dab2d62 84 temp += DACOUTPUT_MIN;
sift 0:276c1dab2d62 85 */
sift 0:276c1dab2d62 86
sift 0:276c1dab2d62 87 temp = (aps * (1.1f/(APS_MAX_VOLTAGE-APS_MIN_VOLTAGE)));
sift 0:276c1dab2d62 88
sift 0:276c1dab2d62 89 return temp/3.3f;
sift 0:276c1dab2d62 90 }
sift 0:276c1dab2d62 91
sift 0:276c1dab2d62 92 Timer timer;
sift 0:276c1dab2d62 93
sift 0:276c1dab2d62 94 Ticker ticker1;
sift 0:276c1dab2d62 95 Ticker ticker2;
sift 0:276c1dab2d62 96
sift 0:276c1dab2d62 97 void generatePulse()
sift 0:276c1dab2d62 98 {
sift 0:276c1dab2d62 99 static bool flag = false;
sift 0:276c1dab2d62 100 flag = !flag;
sift 0:276c1dab2d62 101 MotorPulse[0] = MotorPulse[1] = LED[0] = flag;
sift 0:276c1dab2d62 102 }
sift 0:276c1dab2d62 103
sift 0:276c1dab2d62 104 int pulseTime = 0;
sift 0:276c1dab2d62 105
sift 0:276c1dab2d62 106 void countPulse1()
sift 0:276c1dab2d62 107 {
sift 0:276c1dab2d62 108 //Do not use "printf" in interrupt!!!
sift 0:276c1dab2d62 109 static int preTime=0;
sift 0:276c1dab2d62 110 int currentTime = timer.read_ms();
sift 0:276c1dab2d62 111 pulseTime = currentTime - preTime;
sift 0:276c1dab2d62 112 preTime = currentTime;
sift 0:276c1dab2d62 113 }
sift 0:276c1dab2d62 114
sift 0:276c1dab2d62 115 int convertPulseToVelocity(int pulseTime1, int pulseTime2)
sift 0:276c1dab2d62 116 {
sift 0:276c1dab2d62 117 return (int)convPToV_533[(int)((pulseTime1+pulseTime2)/2.0f)];
sift 0:276c1dab2d62 118 }
sift 0:276c1dab2d62 119
sift 0:276c1dab2d62 120 int main(void)
sift 0:276c1dab2d62 121 {
sift 0:276c1dab2d62 122 printf("\r\nVersion:TVDCU_Alpha...start!!!!!\r\n");
sift 0:276c1dab2d62 123
sift 0:276c1dab2d62 124 initIO(); //IOポート初期化
sift 0:276c1dab2d62 125 initIoExp(); //エキスパンダIO初期化
sift 0:276c1dab2d62 126
sift 0:276c1dab2d62 127 timer.reset();
sift 0:276c1dab2d62 128 timer.start();
sift 0:276c1dab2d62 129
sift 0:276c1dab2d62 130 //ticker.attach(&loadSensorsLPF, 0.001); //サンプリング周期1msec
sift 0:276c1dab2d62 131 ticker1.attach(&generatePulse, 0.001);
sift 0:276c1dab2d62 132
sift 0:276c1dab2d62 133 motorPulse1.mode(PullUp);
sift 0:276c1dab2d62 134 motorPulse1.fall(&countPulse1);
sift 0:276c1dab2d62 135
sift 0:276c1dab2d62 136 bootSystem(); //システム起動
sift 0:276c1dab2d62 137
sift 0:276c1dab2d62 138 indicateSystem(1); //System OK!
sift 0:276c1dab2d62 139
sift 0:276c1dab2d62 140 while(1) {
sift 0:276c1dab2d62 141 printf("%f\r\n", convertPulseToVelocity(pulseTime, pulseTime)*3.6f);
sift 0:276c1dab2d62 142 }
sift 0:276c1dab2d62 143 }
sift 0:276c1dab2d62 144
sift 0:276c1dab2d62 145 void initIO(void)
sift 0:276c1dab2d62 146 {
sift 0:276c1dab2d62 147 indicatorLed = 0;
sift 0:276c1dab2d62 148 shutDown = 0;
sift 0:276c1dab2d62 149 LED[0] = LED[1] = LED[2] = LED[3] = 0;
sift 0:276c1dab2d62 150
sift 0:276c1dab2d62 151 RTDSW.mode(PullUp);
sift 0:276c1dab2d62 152 SDState.mode(PullUp);
sift 0:276c1dab2d62 153 }
sift 0:276c1dab2d62 154
sift 0:276c1dab2d62 155 int checkSensorPlausibility(void)
sift 0:276c1dab2d62 156 {
sift 0:276c1dab2d62 157 int plausibility = 1;
sift 0:276c1dab2d62 158
sift 0:276c1dab2d62 159 if(apsPVol < APS_MIN_VOLTAGE) {
sift 0:276c1dab2d62 160 plausibility = 0;
sift 0:276c1dab2d62 161 LED[0] = 1;
sift 0:276c1dab2d62 162 }
sift 0:276c1dab2d62 163 if(apsPVol > APS_MAX_VOLTAGE) {
sift 0:276c1dab2d62 164 plausibility = 0;
sift 0:276c1dab2d62 165 LED[0] = 1;
sift 0:276c1dab2d62 166 }
sift 0:276c1dab2d62 167 if(apsSVol < APS_MIN_VOLTAGE) {
sift 0:276c1dab2d62 168 plausibility = 0;
sift 0:276c1dab2d62 169 LED[1] = 1;
sift 0:276c1dab2d62 170 }
sift 0:276c1dab2d62 171 if(apsSVol > APS_MAX_VOLTAGE) {
sift 0:276c1dab2d62 172 plausibility = 0;
sift 0:276c1dab2d62 173 LED[1] = 1;
sift 0:276c1dab2d62 174 }
sift 0:276c1dab2d62 175
sift 0:276c1dab2d62 176 return plausibility;
sift 0:276c1dab2d62 177 }
sift 0:276c1dab2d62 178
sift 0:276c1dab2d62 179 void loadSensorsLPF(void)
sift 0:276c1dab2d62 180 {
sift 0:276c1dab2d62 181 //timer.reset();
sift 0:276c1dab2d62 182
sift 0:276c1dab2d62 183 static float preApsP, preApsS;
sift 0:276c1dab2d62 184
sift 0:276c1dab2d62 185 preApsP = apsPVol*ratioLPF + preApsP*(1.0f-ratioLPF);
sift 0:276c1dab2d62 186 preApsS = apsSVol*ratioLPF + preApsS*(1.0f-ratioLPF);
sift 0:276c1dab2d62 187
sift 0:276c1dab2d62 188 // preApsP = apsPVol;
sift 0:276c1dab2d62 189 // preApsS = apsSVol;
sift 0:276c1dab2d62 190
sift 0:276c1dab2d62 191 apsPSample = preApsP;
sift 0:276c1dab2d62 192 apsSSample = preApsS;
sift 0:276c1dab2d62 193 }