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@32:9688c30ac38b, 2017-07-27 (annotated)

Committer:
sift
Date:
Thu Jul 27 15:29:22 2017 +0000
Revision:
32:9688c30ac38b
Parent:
31:042c08a7434f
Child:
34:594ddb4008b2
???G????????????????????????; ????????????????????????????????????

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 24:1de0291bc5eb 26 InterruptIn rightTirePulse1(p24);
sift 24:1de0291bc5eb 27 InterruptIn rightTirePulse2(p25);
sift 24:1de0291bc5eb 28 InterruptIn leftTirePulse1(p26);
sift 24:1de0291bc5eb 29 InterruptIn leftTirePulse2(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 28:47e9531a3a9d 46 #define CANID_MOTORTRQ 0xF3
sift 28:47e9531a3a9d 47 #define CANID_STEERING 0xF4
sift 28:47e9531a3a9d 48 #define CANID_WHEELSPEED 0xF5
sift 28:47e9531a3a9d 49 #define CANID_MOTORSPEED 0xF6
sift 28:47e9531a3a9d 50 #define CANID_VEHICLESPEED 0xF7
sift 31:042c08a7434f 51 #define CANID_DISTRQ 0xF8
sift 28:47e9531a3a9d 52 //++++++++++++++++++++++++++
sift 28:47e9531a3a9d 53
sift 28:47e9531a3a9d 54 //++++++++++++++++++++++++++
sift 27:37a8b4f6d28d 55 //CANデータのバイト数
sift 28:47e9531a3a9d 56 #define CANDATANUM_BRAKE 1
sift 28:47e9531a3a9d 57 #define CANDATANUM_REQTRQ 4
sift 28:47e9531a3a9d 58 #define CANDATANUM_MOTORTRQ 8
sift 28:47e9531a3a9d 59 #define CANDATANUM_STEERING 1
sift 28:47e9531a3a9d 60 #define CANDATANUM_WHEELSPEED 4
sift 28:47e9531a3a9d 61 #define CANDATANUM_MOTORSPEED 4
sift 28:47e9531a3a9d 62 #define CANDATANUM_VEHICLESPEED 2
sift 31:042c08a7434f 63 #define CANDATANUM_DISTRQ 2
sift 27:37a8b4f6d28d 64 //++++++++++++++++++++++++++
sift 2:9d69f27a3d3b 65
sift 28:47e9531a3a9d 66 void writeCanBuff(int data, char* buff, int byteNum)
sift 28:47e9531a3a9d 67 {
sift 28:47e9531a3a9d 68 for(int i=0; i<byteNum; i++) {
sift 28:47e9531a3a9d 69 buff[i] = 0xff & (data >> (i*8));
sift 28:47e9531a3a9d 70 }
sift 28:47e9531a3a9d 71 }
sift 28:47e9531a3a9d 72
sift 27:37a8b4f6d28d 73 void init(void)
sift 0:276c1dab2d62 74 {
sift 0:276c1dab2d62 75 indicatorLed = 0;
sift 0:276c1dab2d62 76 shutDown = 0;
sift 25:c21d35c7f0de 77 brakeSignal = 0;
sift 0:276c1dab2d62 78 LED[0] = LED[1] = LED[2] = LED[3] = 0;
sift 0:276c1dab2d62 79
sift 25:c21d35c7f0de 80 sdState.mode(PullNone);
sift 25:c21d35c7f0de 81 RTDSW.mode(PullNone);
sift 25:c21d35c7f0de 82 SLCTSW[0].mode(PullNone);
sift 25:c21d35c7f0de 83 SLCTSW[1].mode(PullNone);
sift 25:c21d35c7f0de 84 SLCTSW[2].mode(PullNone);
sift 26:331e77bb479b 85
sift 25:c21d35c7f0de 86 rightMotorPulse.mode(PullNone);
sift 25:c21d35c7f0de 87 leftMotorPulse.mode(PullNone);
sift 25:c21d35c7f0de 88 rightTirePulse1.mode(PullNone);
sift 25:c21d35c7f0de 89 rightTirePulse2.mode(PullNone);
sift 25:c21d35c7f0de 90 leftTirePulse1.mode(PullNone);
sift 25:c21d35c7f0de 91 leftTirePulse2.mode(PullNone);
sift 26:331e77bb479b 92
sift 27:37a8b4f6d28d 93 can.frequency(500000); //500kHz
sift 27:37a8b4f6d28d 94
sift 2:9d69f27a3d3b 95 indicateSystem(1);
sift 2:9d69f27a3d3b 96 bootSystem();
sift 0:276c1dab2d62 97 }
sift 0:276c1dab2d62 98
sift 1:4d86ec2fe4b1 99 int main(void)
sift 0:276c1dab2d62 100 {
sift 28:47e9531a3a9d 101 char canBrake[CANDATANUM_BRAKE]= {0};
sift 28:47e9531a3a9d 102 char canRequestTorque[CANDATANUM_REQTRQ]= {0};
sift 28:47e9531a3a9d 103 char canMotorTorque[CANDATANUM_MOTORTRQ]= {0};
sift 28:47e9531a3a9d 104 char canSteer[CANDATANUM_STEERING]= {0};
sift 28:47e9531a3a9d 105 char canMotorSpeed[CANDATANUM_MOTORSPEED]= {0};
sift 28:47e9531a3a9d 106 char canWheelSpeed[CANDATANUM_WHEELSPEED]= {0};
sift 28:47e9531a3a9d 107 char canVehicleSpeed[CANDATANUM_VEHICLESPEED]= {0};
sift 31:042c08a7434f 108 char canTorqueDistribution[CANDATANUM_DISTRQ]= {0};
sift 26:331e77bb479b 109
sift 31:042c08a7434f 110 int INPN_test = -1733;
sift 31:042c08a7434f 111
sift 31:042c08a7434f 112 //wait(1); //断線検出に引っかかるためMotorControllerの起動よりも早くTVDCは起動する必要あり
sift 31:042c08a7434f 113
sift 25:c21d35c7f0de 114 pc.baud(115200);
sift 26:331e77bb479b 115 printf("\033[2J"); //teratermの画面クリア
sift 25:c21d35c7f0de 116 printf("\033[1;1H"); //teratermの画面クリア
sift 25:c21d35c7f0de 117 printf("\r\nVersion:TVDctrller2017_brdRev1...start!!!!!\r\n");
sift 1:4d86ec2fe4b1 118
sift 27:37a8b4f6d28d 119 init(); //IOポート初期化
sift 0:276c1dab2d62 120
sift 1:4d86ec2fe4b1 121 initTVD();
sift 1:4d86ec2fe4b1 122
sift 2:9d69f27a3d3b 123 initSteering();
sift 2:9d69f27a3d3b 124
sift 2:9d69f27a3d3b 125 timer.start();
sift 12:ae291fa7239c 126
sift 2:9d69f27a3d3b 127 float time;
sift 2:9d69f27a3d3b 128
sift 12:ae291fa7239c 129 struct errCounter_t eCounter= {0,0,0,0,0,0,0,0};
sift 26:331e77bb479b 130
sift 29:a51cb2cf22ae 131 wait(3);
sift 26:331e77bb479b 132
sift 1:4d86ec2fe4b1 133 while(1) {
sift 30:c596a0f5d685 134 getCurrentErrCount(&eCounter);
sift 26:331e77bb479b 135
sift 2:9d69f27a3d3b 136 timer.reset();
sift 1:4d86ec2fe4b1 137
sift 26:331e77bb479b 138 driveTVD(0, isPressedRTD());
sift 31:042c08a7434f 139
sift 30:c596a0f5d685 140 brakeSignal.write(isBrakeOn());
sift 6:26fa8c78500e 141
sift 31:042c08a7434f 142 writeCanBuff((int)(getVelocity() / LSB_MOTORSPEED),canVehicleSpeed,CANDATANUM_VEHICLESPEED);
sift 31:042c08a7434f 143 can.write(CANMessage(CANID_VEHICLESPEED, canVehicleSpeed, CANDATANUM_VEHICLESPEED));
sift 31:042c08a7434f 144 writeCanBuff(getSteerAngle()+127, canSteer, CANDATANUM_STEERING);
sift 28:47e9531a3a9d 145 can.write(CANMessage(CANID_STEERING, canSteer, CANDATANUM_STEERING));
sift 30:c596a0f5d685 146 writeCanBuff(calcRequestTorque(), canRequestTorque, CANDATANUM_REQTRQ);
sift 30:c596a0f5d685 147 can.write(CANMessage(CANID_REQTRQ, canRequestTorque, CANDATANUM_REQTRQ));
sift 31:042c08a7434f 148
sift 2:9d69f27a3d3b 149 time = timer.read();
sift 25:c21d35c7f0de 150 while(timer.read_ms() < CONTROL_CYCLE_MS); //制御周期管理 関数内処理時間より短い時間の制御周期の設定は禁止
sift 26:331e77bb479b 151
sift 13:6dc51981f391 152 //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 25:c21d35c7f0de 153 //printf("apsP:%1.2f, apsS:%1.2f, brake:%1.2f\r", 3.3f/65535 * getRawSensor(APS_PRIMARY), 3.3f/65535 * getRawSensor(APS_SECONDARY), 3.3f/65535 * getRawSensor(BRAKE));
sift 0:276c1dab2d62 154 }
sift 2:9d69f27a3d3b 155 }