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

main.cpp@59:7fdf1168c5c3, 2018-02-27 (annotated)

Committer:
sift
Date:
Tue Feb 27 04:35:43 2018 +0000
Revision:
59:7fdf1168c5c3
Parent:
47:949e6c2e69fc 
?ver???????; ???TVDC2018?????????WDT??????????

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 2:9d69f27a3d3b 3 #include "Steering.h"
sift 0:276c1dab2d62 4 #include "MCP4922.h"
sift 25:c21d35c7f0de 5 #include "Global.h"
sift 0:276c1dab2d62 6
sift 2:9d69f27a3d3b 7 Serial pc(USBTX, USBRX); // tx, rx
sift 2:9d69f27a3d3b 8
sift 0:276c1dab2d62 9 ////////////////////////////////////////
sift 0:276c1dab2d62 10 //IO宣言
sift 0:276c1dab2d62 11 SPI spi(p5,p6,p7);
sift 24:1de0291bc5eb 12 MCP4922 mcp(p5,p7,p8); // MOSI, SCLK, CS
sift 24:1de0291bc5eb 13 DigitalOut ioExpCs(p9);
sift 24:1de0291bc5eb 14 DigitalOut indicatorLed(p10);
sift 24:1de0291bc5eb 15 DigitalOut shutDown(p11);
sift 24:1de0291bc5eb 16 DigitalOut brakeSignal(p12);
sift 25:c21d35c7f0de 17 DigitalIn sdState(p13);
sift 24:1de0291bc5eb 18 DigitalIn RTDSW(p14);
sift 24:1de0291bc5eb 19 AnalogIn apsS(p15); //"S"econdary
sift 24:1de0291bc5eb 20 AnalogIn apsP(p16); //"P"rimary
sift 0:276c1dab2d62 21 AnalogIn brake(p17);
sift 19:571a4d00b89c 22 AnalogOut STR2AN(p18);
sift 24:1de0291bc5eb 23 DigitalIn SLCTSW[3] = {p19, p20, p21};
sift 24:1de0291bc5eb 24 InterruptIn rightMotorPulse(p22);
sift 24:1de0291bc5eb 25 InterruptIn leftMotorPulse(p23);
sift 39:c05074379713 26 InterruptIn rightWheelPulse1(p24);
sift 39:c05074379713 27 InterruptIn rightWheelPulse2(p25);
sift 39:c05074379713 28 InterruptIn leftWheelPulse1(p26);
sift 39:c05074379713 29 InterruptIn leftWheelPulse2(p27);
sift 24:1de0291bc5eb 30 DigitalOut WDT(p28);
sift 19:571a4d00b89c 31 CAN can(p30, p29);
sift 27:37a8b4f6d28d 32 DigitalOut LED[] = {LED1, LED2, LED3, LED4};
sift 0:276c1dab2d62 33
sift 27:37a8b4f6d28d 34 Timer timer;
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 27:37a8b4f6d28d 42 //++++++++++++++++++++++++++
sift 28:47e9531a3a9d 43 //CAN ID
sift 28:47e9531a3a9d 44 #define CANID_BRAKE 0xF1
sift 28:47e9531a3a9d 45 #define CANID_REQTRQ 0xF2
sift 38:11753ee9734f 46 #define CANID_RIGHT_TRQ 0xF3
sift 38:11753ee9734f 47 #define CANID_LEFT_TRQ 0xF4
sift 38:11753ee9734f 48 #define CANID_STEERING 0xF5
sift 38:11753ee9734f 49 #define CANID_WHEELSPEED 0xF6
sift 38:11753ee9734f 50 #define CANID_MOTORSPEED 0xF7
sift 38:11753ee9734f 51 #define CANID_VEHICLESPEED 0xF8
sift 38:11753ee9734f 52 #define CANID_DISTRQ 0xF9
sift 44:d433bb5f77c0 53 #define CANID_RR_RPS 0xFA
sift 44:d433bb5f77c0 54 #define CANID_RL_RPS 0xFB
sift 44:d433bb5f77c0 55 #define CANID_FR_RPS 0xFC
sift 44:d433bb5f77c0 56 #define CANID_FL_RPS 0xFD
sift 44:d433bb5f77c0 57 #define CANID_APS_P 0xFE
sift 44:d433bb5f77c0 58 #define CANID_APS_S 0xFF
sift 28:47e9531a3a9d 59 //++++++++++++++++++++++++++
sift 28:47e9531a3a9d 60
sift 28:47e9531a3a9d 61 //++++++++++++++++++++++++++
sift 27:37a8b4f6d28d 62 //CANデータのバイト数
sift 28:47e9531a3a9d 63 #define CANDATANUM_BRAKE 1
sift 28:47e9531a3a9d 64 #define CANDATANUM_REQTRQ 4
sift 38:11753ee9734f 65 #define CANDATANUM_MOTORTRQ 4
sift 28:47e9531a3a9d 66 #define CANDATANUM_STEERING 1
sift 28:47e9531a3a9d 67 #define CANDATANUM_WHEELSPEED 4
sift 28:47e9531a3a9d 68 #define CANDATANUM_MOTORSPEED 4
sift 28:47e9531a3a9d 69 #define CANDATANUM_VEHICLESPEED 2
sift 31:042c08a7434f 70 #define CANDATANUM_DISTRQ 2
sift 44:d433bb5f77c0 71 #define CANDATANUM_RPS 2
sift 44:d433bb5f77c0 72 #define CANDATANUM_APS 2
sift 27:37a8b4f6d28d 73 //++++++++++++++++++++++++++
sift 2:9d69f27a3d3b 74
sift 34:594ddb4008b2 75 void writeCanBuff(long long data, char* buff, int byteNum)
sift 28:47e9531a3a9d 76 {
sift 28:47e9531a3a9d 77 for(int i=0; i<byteNum; i++) {
sift 28:47e9531a3a9d 78 buff[i] = 0xff & (data >> (i*8));
sift 28:47e9531a3a9d 79 }
sift 28:47e9531a3a9d 80 }
sift 28:47e9531a3a9d 81
sift 27:37a8b4f6d28d 82 void init(void)
sift 0:276c1dab2d62 83 {
sift 0:276c1dab2d62 84 indicatorLed = 0;
sift 0:276c1dab2d62 85 shutDown = 0;
sift 25:c21d35c7f0de 86 brakeSignal = 0;
sift 0:276c1dab2d62 87 LED[0] = LED[1] = LED[2] = LED[3] = 0;
sift 0:276c1dab2d62 88
sift 25:c21d35c7f0de 89 sdState.mode(PullNone);
sift 25:c21d35c7f0de 90 RTDSW.mode(PullNone);
sift 25:c21d35c7f0de 91 SLCTSW[0].mode(PullNone);
sift 25:c21d35c7f0de 92 SLCTSW[1].mode(PullNone);
sift 25:c21d35c7f0de 93 SLCTSW[2].mode(PullNone);
sift 26:331e77bb479b 94
sift 25:c21d35c7f0de 95 rightMotorPulse.mode(PullNone);
sift 25:c21d35c7f0de 96 leftMotorPulse.mode(PullNone);
sift 39:c05074379713 97 rightWheelPulse1.mode(PullNone);
sift 39:c05074379713 98 rightWheelPulse2.mode(PullNone);
sift 39:c05074379713 99 leftWheelPulse1.mode(PullNone);
sift 39:c05074379713 100 leftWheelPulse2.mode(PullNone);
sift 26:331e77bb479b 101
sift 27:37a8b4f6d28d 102 can.frequency(500000); //500kHz
sift 27:37a8b4f6d28d 103
sift 2:9d69f27a3d3b 104 indicateSystem(1);
sift 2:9d69f27a3d3b 105 bootSystem();
sift 0:276c1dab2d62 106 }
sift 0:276c1dab2d62 107
sift 1:4d86ec2fe4b1 108 int main(void)
sift 0:276c1dab2d62 109 {
sift 59:7fdf1168c5c3 110 bool wdtFlag = false;
sift 39:c05074379713 111 // char canBrake[CANDATANUM_BRAKE]= {0};
sift 28:47e9531a3a9d 112 char canRequestTorque[CANDATANUM_REQTRQ]= {0};
sift 38:11753ee9734f 113 char canMotorTorqueR[CANDATANUM_MOTORTRQ]= {0};
sift 38:11753ee9734f 114 char canMotorTorqueL[CANDATANUM_MOTORTRQ]= {0};
sift 28:47e9531a3a9d 115 char canSteer[CANDATANUM_STEERING]= {0};
sift 39:c05074379713 116 // char canMotorSpeed[CANDATANUM_MOTORSPEED]= {0};
sift 39:c05074379713 117 // char canWheelSpeed[CANDATANUM_WHEELSPEED]= {0};
sift 28:47e9531a3a9d 118 char canVehicleSpeed[CANDATANUM_VEHICLESPEED]= {0};
sift 44:d433bb5f77c0 119 char canTorqueDistribution[CANDATANUM_DISTRQ]= {0};
sift 44:d433bb5f77c0 120 char canRR_RPS[CANDATANUM_RPS]= {0};
sift 44:d433bb5f77c0 121 char canRL_RPS[CANDATANUM_RPS]= {0};
sift 44:d433bb5f77c0 122 char canFR_RPS[CANDATANUM_RPS]= {0};
sift 44:d433bb5f77c0 123 char canFL_RPS[CANDATANUM_RPS]= {0};
sift 44:d433bb5f77c0 124 char canAPS_P[CANDATANUM_APS]= {0};
sift 44:d433bb5f77c0 125 char canAPS_S[CANDATANUM_APS]= {0};
sift 26:331e77bb479b 126
sift 44:d433bb5f77c0 127 unsigned int canWriteSucceeded = 0;
sift 31:042c08a7434f 128
sift 31:042c08a7434f 129 //wait(1); //断線検出に引っかかるためMotorControllerの起動よりも早くTVDCは起動する必要あり
sift 31:042c08a7434f 130
sift 25:c21d35c7f0de 131 pc.baud(115200);
sift 26:331e77bb479b 132 printf("\033[2J"); //teratermの画面クリア
sift 25:c21d35c7f0de 133 printf("\033[1;1H"); //teratermの画面クリア
sift 25:c21d35c7f0de 134 printf("\r\nVersion:TVDctrller2017_brdRev1...start!!!!!\r\n");
sift 1:4d86ec2fe4b1 135
sift 27:37a8b4f6d28d 136 init(); //IOポート初期化
sift 0:276c1dab2d62 137
sift 1:4d86ec2fe4b1 138 initTVD();
sift 1:4d86ec2fe4b1 139
sift 2:9d69f27a3d3b 140 initSteering();
sift 2:9d69f27a3d3b 141
sift 2:9d69f27a3d3b 142 timer.start();
sift 12:ae291fa7239c 143
sift 12:ae291fa7239c 144 struct errCounter_t eCounter= {0,0,0,0,0,0,0,0};
sift 26:331e77bb479b 145
sift 29:a51cb2cf22ae 146 wait(3);
sift 26:331e77bb479b 147
sift 1:4d86ec2fe4b1 148 while(1) {
sift 59:7fdf1168c5c3 149
sift 59:7fdf1168c5c3 150 wdtFlag = !wdtFlag;
sift 59:7fdf1168c5c3 151 WDT = wdtFlag;
sift 34:594ddb4008b2 152
sift 34:594ddb4008b2 153 canWriteSucceeded = 0;
sift 34:594ddb4008b2 154
sift 30:c596a0f5d685 155 getCurrentErrCount(&eCounter);
sift 26:331e77bb479b 156
sift 2:9d69f27a3d3b 157 timer.reset();
sift 1:4d86ec2fe4b1 158
sift 26:331e77bb479b 159 driveTVD(0, isPressedRTD());
sift 31:042c08a7434f 160
sift 30:c596a0f5d685 161 brakeSignal.write(isBrakeOn());
sift 6:26fa8c78500e 162
sift 31:042c08a7434f 163 writeCanBuff((int)(getVelocity() / LSB_MOTORSPEED),canVehicleSpeed,CANDATANUM_VEHICLESPEED);
sift 31:042c08a7434f 164 writeCanBuff(getSteerAngle()+127, canSteer, CANDATANUM_STEERING);
sift 30:c596a0f5d685 165 writeCanBuff(calcRequestTorque(), canRequestTorque, CANDATANUM_REQTRQ);
sift 43:5da6b1574227 166 writeCanBuff(getMotorTorque(RR_MOTOR), canMotorTorqueR, CANDATANUM_MOTORTRQ);
sift 43:5da6b1574227 167 writeCanBuff(getMotorTorque(RL_MOTOR), canMotorTorqueL, CANDATANUM_MOTORTRQ);
sift 34:594ddb4008b2 168
sift 44:d433bb5f77c0 169 writeCanBuff(getWheelRps(FR_WHEEL) / LSB_RPS, canFR_RPS, CANDATANUM_RPS);
sift 44:d433bb5f77c0 170 writeCanBuff(getWheelRps(FL_WHEEL) / LSB_RPS, canFL_RPS, CANDATANUM_RPS);
sift 44:d433bb5f77c0 171 writeCanBuff(getWheelRps(RR_MOTOR) / LSB_RPS, canRR_RPS, CANDATANUM_RPS);
sift 44:d433bb5f77c0 172 writeCanBuff(getWheelRps(RL_MOTOR) / LSB_RPS, canRL_RPS, CANDATANUM_RPS);
sift 44:d433bb5f77c0 173
sift 44:d433bb5f77c0 174 writeCanBuff(getRawSensor(APS_PRIMARY), canAPS_P, CANDATANUM_APS);
sift 44:d433bb5f77c0 175 writeCanBuff(getRawSensor(APS_SECONDARY), canAPS_S, CANDATANUM_APS);
sift 44:d433bb5f77c0 176
sift 47:949e6c2e69fc 177 // printf("%d %d %d %d %d\r\n", getMotorTorque(RR_MOTOR), getMotorTorque(RL_MOTOR), calcRequestTorque(), 0, 4000);
sift 44:d433bb5f77c0 178
sift 44:d433bb5f77c0 179 // writeCanBuff((int)((distributeTorque(M_PI * getSteerAngle() / 127.0f, getVelocity())*limitTorqueDistribution()) / 2.0f), canTorqueDistribution, CANDATANUM_DISTRQ);
sift 44:d433bb5f77c0 180
sift 34:594ddb4008b2 181 do {
sift 44:d433bb5f77c0 182 if(0 == (canWriteSucceeded & 0b1))
sift 44:d433bb5f77c0 183 canWriteSucceeded |= (can.write(CANMessage(CANID_VEHICLESPEED, canVehicleSpeed, CANDATANUM_VEHICLESPEED)) << 0);
sift 44:d433bb5f77c0 184 if(0 == (canWriteSucceeded & 0b10))
sift 44:d433bb5f77c0 185 canWriteSucceeded |= (can.write(CANMessage(CANID_STEERING, canSteer, CANDATANUM_STEERING)) << 1);
sift 44:d433bb5f77c0 186 if(0 == (canWriteSucceeded & 0b100))
sift 44:d433bb5f77c0 187 canWriteSucceeded |= (can.write(CANMessage(CANID_REQTRQ, canRequestTorque, CANDATANUM_REQTRQ)) << 2);
sift 44:d433bb5f77c0 188 if(0 == (canWriteSucceeded & 0b1000))
sift 44:d433bb5f77c0 189 canWriteSucceeded |= (can.write(CANMessage(CANID_RIGHT_TRQ, canMotorTorqueR, CANDATANUM_MOTORTRQ)) << 3);
sift 44:d433bb5f77c0 190 if(0 == (canWriteSucceeded & 0b10000))
sift 44:d433bb5f77c0 191 canWriteSucceeded |= (can.write(CANMessage(CANID_LEFT_TRQ, canMotorTorqueL, CANDATANUM_MOTORTRQ)) << 4);
sift 44:d433bb5f77c0 192
sift 44:d433bb5f77c0 193 if(0 == (canWriteSucceeded & 0b100000))
sift 44:d433bb5f77c0 194 canWriteSucceeded |= (can.write(CANMessage(CANID_FR_RPS, canFR_RPS, CANDATANUM_RPS)) << 5);
sift 44:d433bb5f77c0 195 if(0 == (canWriteSucceeded & 0b1000000))
sift 44:d433bb5f77c0 196 canWriteSucceeded |= (can.write(CANMessage(CANID_FL_RPS, canFL_RPS, CANDATANUM_RPS)) << 6);
sift 44:d433bb5f77c0 197 if(0 == (canWriteSucceeded & 0b10000000))
sift 44:d433bb5f77c0 198 canWriteSucceeded |= (can.write(CANMessage(CANID_RR_RPS, canRR_RPS, CANDATANUM_RPS)) << 7);
sift 44:d433bb5f77c0 199 if(0 == (canWriteSucceeded & 0b100000000))
sift 44:d433bb5f77c0 200 canWriteSucceeded |= (can.write(CANMessage(CANID_RL_RPS, canRL_RPS, CANDATANUM_RPS)) << 8);
sift 44:d433bb5f77c0 201 if(0 == (canWriteSucceeded & 0b1000000000))
sift 44:d433bb5f77c0 202 canWriteSucceeded |= (can.write(CANMessage(CANID_DISTRQ, canTorqueDistribution, CANDATANUM_DISTRQ)) << 9);
sift 44:d433bb5f77c0 203
sift 44:d433bb5f77c0 204 if(0 == (canWriteSucceeded & 0b10000000000))
sift 44:d433bb5f77c0 205 canWriteSucceeded |= (can.write(CANMessage(CANID_APS_P, canAPS_P, CANDATANUM_APS)) << 10);
sift 44:d433bb5f77c0 206 if(0 == (canWriteSucceeded & 0b100000000000))
sift 44:d433bb5f77c0 207 canWriteSucceeded |= (can.write(CANMessage(CANID_APS_S, canAPS_S, CANDATANUM_APS)) << 11);
sift 44:d433bb5f77c0 208 //printf("%d\r\n", canWriteSucceeded);
sift 43:5da6b1574227 209 } while(timer.read() < CONTROL_CYCLE_S); //制御周期管理 関数内処理時間より短い時間の制御周期の設定は禁止
sift 34:594ddb4008b2 210 //printf("\r\n");
sift 26:331e77bb479b 211
sift 13:6dc51981f391 212 //printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\r\n", eCounter.apsUnderVolt, eCounter.apsExceedVolt, eCounter.apsErrorTolerance, eCounter.apsStick, eCounter.brakeUnderVolt, eCounter.brakeExceedVolt, eCounter.brakeFuzzyVolt, eCounter.brakeOverRide);
sift 38:11753ee9734f 213 // printf("apsP:%1.2f, apsS:%1.2f, brake:%1.2f,RPS_R:%1.2f, RPS_L%1.2f\r", 3.3f/65535 * getRawSensor(APS_PRIMARY), 3.3f/65535 * getRawSensor(APS_SECONDARY), 3.3f/65535 * getRawSensor(BRAKE), getRPS(RIGHT)*LSB_MOTORSPEED*GEAR_RATIO*60.0f, getRPS(LEFT)*LSB_MOTORSPEED*GEAR_RATIO*60.0f);
sift 0:276c1dab2d62 214 }
sift 2:9d69f27a3d3b 215 }