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

TVDCTRL.cpp@1:4d86ec2fe4b1, 2016-07-09 (annotated)

Committer:
sift
Date:
Sat Jul 09 12:04:47 2016 +0000
Revision:
1:4d86ec2fe4b1
Parent:
0:276c1dab2d62
Child:
2:9d69f27a3d3b
??????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sift 0:276c1dab2d62 1 #include "TVDCTRL.h"
sift 1:4d86ec2fe4b1 2 #include "MCP4922.h"
sift 1:4d86ec2fe4b1 3 #include "Steering.h"
sift 1:4d86ec2fe4b1 4
sift 1:4d86ec2fe4b1 5 extern AnalogIn apsP;
sift 1:4d86ec2fe4b1 6 extern AnalogIn apsS;
sift 1:4d86ec2fe4b1 7 extern DigitalOut LED[];
sift 1:4d86ec2fe4b1 8 extern InterruptIn rightMotorPulse;
sift 1:4d86ec2fe4b1 9 extern InterruptIn leftMotorPulse;
sift 1:4d86ec2fe4b1 10 extern DigitalOut MotorPulse[];
sift 1:4d86ec2fe4b1 11 extern MCP4922 mcp;
sift 1:4d86ec2fe4b1 12
sift 1:4d86ec2fe4b1 13 static Ticker ticker; //static -> このファイル内でのみ有効
sift 1:4d86ec2fe4b1 14 static Timer timer;
sift 1:4d86ec2fe4b1 15
sift 1:4d86ec2fe4b1 16 #define apsPVol() (apsP.read() * 3.3f)
sift 1:4d86ec2fe4b1 17 #define apsSVol() (apsS.read() * 3.3f)
sift 1:4d86ec2fe4b1 18
sift 1:4d86ec2fe4b1 19 const unsigned int APS_MIN_POSITION =(unsigned int)(0xFFFF/3.3f * 1.0f); //"正常時"最小入力電圧
sift 1:4d86ec2fe4b1 20 const unsigned int APS_MAX_POSITION =(unsigned int)(0xFFFF/3.3f * 1.27f); //"正常時"最大入力電圧
sift 1:4d86ec2fe4b1 21 const unsigned int APS_LOW_VOLTAGE =(unsigned int)(0xFFFF/3.3f * 0.35f); //"異常時"最低入力電圧
sift 1:4d86ec2fe4b1 22 const unsigned int APS_HIGH_VOLTAGE =(unsigned int)(0xFFFF/3.3f * 3.0f); //"異常時"最高入力電圧
sift 1:4d86ec2fe4b1 23 const unsigned int APS_DEADBAND =(unsigned int)((APS_MAX_POSITION - APS_MIN_POSITION) * 0.1f); //APS信号不感帯
sift 1:4d86ec2fe4b1 24 const unsigned int APS_VALID_RANGE =(APS_MAX_POSITION - APS_MIN_POSITION) - APS_DEADBAND; //APS信号有効範囲
sift 1:4d86ec2fe4b1 25
sift 1:4d86ec2fe4b1 26 const unsigned int MCP_REFERENCE =(unsigned int)(0xFFFF/3.3f * 3.3f);
sift 1:4d86ec2fe4b1 27 const unsigned int DACOUTPUT_MIN =(unsigned int)(0xFFFF/3.3f * 0.49f);
sift 1:4d86ec2fe4b1 28 const unsigned int DACOUTPUT_MAX =(unsigned int)(0xFFFF/3.3f * 2.45f);
sift 1:4d86ec2fe4b1 29 const unsigned int DACOUTPUT_VALID_RANGE =DACOUTPUT_MAX - DACOUTPUT_MIN;
sift 1:4d86ec2fe4b1 30 const unsigned int DACOUTPUT_LIMIT =(unsigned int)(0xFFFF/3.3f * 0.4f);
sift 0:276c1dab2d62 31
sift 0:276c1dab2d62 32 struct {
sift 0:276c1dab2d62 33 unsigned int valA:12;
sift 0:276c1dab2d62 34 unsigned int valB:12;
sift 0:276c1dab2d62 35 } mcpData;
sift 0:276c1dab2d62 36
sift 1:4d86ec2fe4b1 37 unsigned int g_apsP=0, g_apsS=0, g_brake=0; //現在のセンサ値
sift 1:4d86ec2fe4b1 38 unsigned int rightTire=0, leftTire=0; //出力タイヤトルク
sift 1:4d86ec2fe4b1 39 unsigned int currentAPS=0;
sift 1:4d86ec2fe4b1 40 unsigned int currentRequestTorque=0;
sift 1:4d86ec2fe4b1 41 unsigned int rightPulseTime=0, leftPulseTime=0;
sift 1:4d86ec2fe4b1 42
sift 1:4d86ec2fe4b1 43 struct {
sift 1:4d86ec2fe4b1 44 unsigned int apsUnderVolt; //aps電圧不足
sift 1:4d86ec2fe4b1 45 unsigned int apsExceedVolt; //aps電圧超過
sift 1:4d86ec2fe4b1 46 unsigned int brakeUnderVolt; //brake電圧不足
sift 1:4d86ec2fe4b1 47 unsigned int brakeExceedVolt; //brake電圧超過
sift 1:4d86ec2fe4b1 48 unsigned int apsStick; //aps固着
sift 1:4d86ec2fe4b1 49 } errCounter={0,0,0,0,0};
sift 1:4d86ec2fe4b1 50
sift 1:4d86ec2fe4b1 51 inline float convertVoltage(unsigned int count)
sift 1:4d86ec2fe4b1 52 {
sift 1:4d86ec2fe4b1 53 return (count*(3.3f/0xFFFF));
sift 1:4d86ec2fe4b1 54 }
sift 1:4d86ec2fe4b1 55
sift 1:4d86ec2fe4b1 56 int checkSensorPlausibility(void)
sift 1:4d86ec2fe4b1 57 {
sift 1:4d86ec2fe4b1 58 int plausibility = 1;
sift 1:4d86ec2fe4b1 59
sift 1:4d86ec2fe4b1 60 if(g_apsP < APS_MIN_POSITION) {
sift 1:4d86ec2fe4b1 61 plausibility = 0;
sift 1:4d86ec2fe4b1 62 LED[0] = 1;
sift 1:4d86ec2fe4b1 63 }
sift 1:4d86ec2fe4b1 64 if(g_apsP > APS_MAX_POSITION) {
sift 1:4d86ec2fe4b1 65 plausibility = 0;
sift 1:4d86ec2fe4b1 66 LED[0] = 1;
sift 1:4d86ec2fe4b1 67 }
sift 1:4d86ec2fe4b1 68 if(g_apsS < APS_MIN_POSITION) {
sift 1:4d86ec2fe4b1 69 plausibility = 0;
sift 1:4d86ec2fe4b1 70 LED[1] = 1;
sift 1:4d86ec2fe4b1 71 }
sift 1:4d86ec2fe4b1 72 if(g_apsS > APS_MAX_POSITION) {
sift 1:4d86ec2fe4b1 73 plausibility = 0;
sift 1:4d86ec2fe4b1 74 LED[1] = 1;
sift 1:4d86ec2fe4b1 75 }
sift 1:4d86ec2fe4b1 76
sift 1:4d86ec2fe4b1 77 return plausibility;
sift 1:4d86ec2fe4b1 78 }
sift 1:4d86ec2fe4b1 79
sift 1:4d86ec2fe4b1 80 inline void calcRequestTorque(void)
sift 1:4d86ec2fe4b1 81 {
sift 1:4d86ec2fe4b1 82 unsigned int currentAPS;
sift 1:4d86ec2fe4b1 83
sift 1:4d86ec2fe4b1 84 currentAPS = ((g_apsP>g_apsS) ? g_apsS : g_apsP); //センサ値は小さい方を採用
sift 1:4d86ec2fe4b1 85 currentAPS -= APS_MIN_POSITION; //オフセット修正
sift 1:4d86ec2fe4b1 86
sift 1:4d86ec2fe4b1 87 if(currentAPS < APS_DEADBAND) //デッドバンド内であれば要求トルク->0
sift 1:4d86ec2fe4b1 88 currentRequestTorque = 0;
sift 1:4d86ec2fe4b1 89 else
sift 1:4d86ec2fe4b1 90 currentRequestTorque = (unsigned int)(((float)MAX_MOTOR_TORQUE / APS_VALID_RANGE) * (currentAPS - APS_DEADBAND));
sift 1:4d86ec2fe4b1 91 }
sift 1:4d86ec2fe4b1 92
sift 1:4d86ec2fe4b1 93 void loadSensorsLPF(void)
sift 1:4d86ec2fe4b1 94 {
sift 1:4d86ec2fe4b1 95 static unsigned int preApsP=0, preApsS=0;
sift 1:4d86ec2fe4b1 96
sift 1:4d86ec2fe4b1 97 preApsP = (unsigned int)(apsP.read_u16()*ratioLPF + preApsP*(1.0f-ratioLPF));
sift 1:4d86ec2fe4b1 98 preApsS = (unsigned int)(apsS.read_u16()*ratioLPF + preApsS*(1.0f-ratioLPF));
sift 1:4d86ec2fe4b1 99
sift 1:4d86ec2fe4b1 100 g_apsP = preApsP;
sift 1:4d86ec2fe4b1 101 g_apsS = preApsS;
sift 1:4d86ec2fe4b1 102 }
sift 1:4d86ec2fe4b1 103
sift 1:4d86ec2fe4b1 104 int getVelocity(void)
sift 1:4d86ec2fe4b1 105 {
sift 1:4d86ec2fe4b1 106 if(rightPulseTime > 100)
sift 1:4d86ec2fe4b1 107 rightPulseTime = 100;
sift 1:4d86ec2fe4b1 108 if(leftPulseTime > 100)
sift 1:4d86ec2fe4b1 109 leftPulseTime = 100;
sift 1:4d86ec2fe4b1 110
sift 1:4d86ec2fe4b1 111 //return (int)convPToV_533[(int)((rightPulseTime+leftPulseTime)/2.0f)];
sift 1:4d86ec2fe4b1 112 return (int)convPToV_533[rightPulseTime];
sift 1:4d86ec2fe4b1 113 }
sift 1:4d86ec2fe4b1 114
sift 1:4d86ec2fe4b1 115 void countPulseR(void)
sift 1:4d86ec2fe4b1 116 {
sift 1:4d86ec2fe4b1 117 //Do not use "printf" in interrupt!!!
sift 1:4d86ec2fe4b1 118 static int preTime=0;
sift 1:4d86ec2fe4b1 119 int currentTime = timer.read_ms();
sift 1:4d86ec2fe4b1 120 rightPulseTime = currentTime - preTime;
sift 1:4d86ec2fe4b1 121 preTime = currentTime;
sift 1:4d86ec2fe4b1 122 }
sift 1:4d86ec2fe4b1 123
sift 1:4d86ec2fe4b1 124 void countPulseL(void)
sift 1:4d86ec2fe4b1 125 {
sift 1:4d86ec2fe4b1 126 //Do not use "printf" in interrupt!!!
sift 1:4d86ec2fe4b1 127 static int preTime=0;
sift 1:4d86ec2fe4b1 128 int currentTime = timer.read_ms();
sift 1:4d86ec2fe4b1 129 leftPulseTime = currentTime - preTime;
sift 1:4d86ec2fe4b1 130 preTime = currentTime;
sift 1:4d86ec2fe4b1 131 }
sift 1:4d86ec2fe4b1 132
sift 1:4d86ec2fe4b1 133 void generatePulse(void)
sift 1:4d86ec2fe4b1 134 {
sift 1:4d86ec2fe4b1 135 static bool flag = false;
sift 1:4d86ec2fe4b1 136 flag = !flag;
sift 1:4d86ec2fe4b1 137 MotorPulse[0] = MotorPulse[1] = LED[0] = flag;
sift 1:4d86ec2fe4b1 138 }
sift 1:4d86ec2fe4b1 139
sift 1:4d86ec2fe4b1 140 void driveTVD(void)
sift 1:4d86ec2fe4b1 141 {
sift 1:4d86ec2fe4b1 142 //仮に等配分のみ
sift 1:4d86ec2fe4b1 143 mcpData.valA = (unsigned int)(((float)DACOUTPUT_VALID_RANGE / MAX_MOTOR_TORQUE) * currentRequestTorque) + DACOUTPUT_MIN;
sift 1:4d86ec2fe4b1 144 mcpData.valB = (unsigned int)(((float)DACOUTPUT_VALID_RANGE / MAX_MOTOR_TORQUE) * currentRequestTorque) + DACOUTPUT_MIN;
sift 1:4d86ec2fe4b1 145
sift 1:4d86ec2fe4b1 146 mcp.writeA(mcpData.valA);
sift 1:4d86ec2fe4b1 147 mcp.writeB(mcpData.valB);
sift 1:4d86ec2fe4b1 148 }
sift 1:4d86ec2fe4b1 149
sift 1:4d86ec2fe4b1 150 void initTVD(void)
sift 1:4d86ec2fe4b1 151 {
sift 1:4d86ec2fe4b1 152 rightMotorPulse.mode(PullUp);
sift 1:4d86ec2fe4b1 153 leftMotorPulse.mode(PullUp);
sift 1:4d86ec2fe4b1 154 rightMotorPulse.fall(&countPulseR);
sift 1:4d86ec2fe4b1 155 leftMotorPulse.fall(&countPulseL);
sift 1:4d86ec2fe4b1 156
sift 1:4d86ec2fe4b1 157 timer.reset();
sift 1:4d86ec2fe4b1 158 timer.start();
sift 1:4d86ec2fe4b1 159
sift 1:4d86ec2fe4b1 160 ticker.attach(&loadSensorsLPF, 0.001); //サンプリング周期1msec
sift 1:4d86ec2fe4b1 161 ticker.attach(&generatePulse, 0.025);
sift 1:4d86ec2fe4b1 162 }
sift 1:4d86ec2fe4b1 163
sift 1:4d86ec2fe4b1 164
sift 1:4d86ec2fe4b1 165
sift 1:4d86ec2fe4b1 166
sift 1:4d86ec2fe4b1 167 //APS信号をモーターコントローラの入力電圧に変換
sift 1:4d86ec2fe4b1 168 //fixVoltageを通してから使うこと
sift 1:4d86ec2fe4b1 169 inline float fixSpecifiedOutputData(float aps)
sift 1:4d86ec2fe4b1 170 {
sift 1:4d86ec2fe4b1 171 float temp;
sift 1:4d86ec2fe4b1 172
sift 1:4d86ec2fe4b1 173 aps = ((aps<APS_MIN_POSITION) ? 0.0f : (aps-APS_MIN_POSITION)); //オフセット修正
sift 1:4d86ec2fe4b1 174
sift 1:4d86ec2fe4b1 175 /*
sift 1:4d86ec2fe4b1 176 if(aps < APS_DEADBAND)
sift 1:4d86ec2fe4b1 177 temp = 0.0f;
sift 1:4d86ec2fe4b1 178 else {
sift 1:4d86ec2fe4b1 179 aps -= APS_DEADBAND;
sift 1:4d86ec2fe4b1 180 temp = (aps * (DACOUTPUT_VALID_RANGE/(APS_VALID_RANGE)));
sift 1:4d86ec2fe4b1 181 temp *= LIMIT;
sift 1:4d86ec2fe4b1 182 }
sift 1:4d86ec2fe4b1 183
sift 1:4d86ec2fe4b1 184 temp += DACOUTPUT_MIN;
sift 1:4d86ec2fe4b1 185 */
sift 1:4d86ec2fe4b1 186
sift 1:4d86ec2fe4b1 187 //temp = (aps * (1.1f/(APS_MAX_VOLTAGE-APS_MIN_VOLTAGE)));
sift 1:4d86ec2fe4b1 188
sift 1:4d86ec2fe4b1 189 return temp/3.3f;
sift 1:4d86ec2fe4b1 190 }
sift 1:4d86ec2fe4b1 191