Prathamesh Joshi / Mbed 2 deprecated EDiffMain

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.

main.cpp@5:ee37889edcab, 2013-03-09 (annotated)

Committer:
prathamesh1729
Date:
Sat Mar 09 06:10:08 2013 +0000
Revision:
5:ee37889edcab
Parent:
4:66b2a94a607f
Child:
6:58bd138f08af
Last commit before end-of-day.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
prathamesh1729 0:cee310e0c668 1 #include "algo.h"
prathamesh1729 0:cee310e0c668 2 #include "fmea.h"
prathamesh1729 1:c0a9790eed5b 3 #include "DAC.h"
prathamesh1729 1:c0a9790eed5b 4 #include "Mux.h"
prathamesh1729 0:cee310e0c668 5
prathamesh1729 0:cee310e0c668 6 // - - - Parameters - - - //
prathamesh1729 0:cee310e0c668 7
prathamesh1729 0:cee310e0c668 8 //car parameters
prathamesh1729 0:cee310e0c668 9 const float trackwidth = 49*0.0254;
prathamesh1729 0:cee310e0c668 10 const float wheelradius = 0.254;
prathamesh1729 0:cee310e0c668 11 const float rear_wheelbase = 0.686816; //rear weightbias*wheelbase
prathamesh1729 0:cee310e0c668 12
prathamesh1729 0:cee310e0c668 13 //constant parameters
prathamesh1729 0:cee310e0c668 14 const float full_throttle=5;
prathamesh1729 0:cee310e0c668 15 const float dead_steering=7;
prathamesh1729 0:cee310e0c668 16 const float integral_saturation=0.5;
prathamesh1729 0:cee310e0c668 17 const float dead_rpm = 10;
prathamesh1729 0:cee310e0c668 18
prathamesh1729 0:cee310e0c668 19 // - - - Inputs - - - //
prathamesh1729 0:cee310e0c668 20
prathamesh1729 1:c0a9790eed5b 21 //steering, throttle inputs
prathamesh1729 0:cee310e0c668 22 float steering,steering2; //input steering wheel angle in degrees
prathamesh1729 0:cee310e0c668 23 float throttle, throttle2; //throttle input from pedal
prathamesh1729 0:cee310e0c668 24 float yaw_rate;
prathamesh1729 3:68d91895d991 25 float brake, brake2;
prathamesh1729 0:cee310e0c668 26
prathamesh1729 0:cee310e0c668 27 // - - - Outputs - - - //
prathamesh1729 0:cee310e0c668 28
prathamesh1729 0:cee310e0c668 29 // throttle output values
prathamesh1729 0:cee310e0c668 30 float throttle_Left, throttle_Right;
prathamesh1729 0:cee310e0c668 31
prathamesh1729 0:cee310e0c668 32 // - - - For Basic Algo and Steering to turnradius function - - - //
prathamesh1729 0:cee310e0c668 33
prathamesh1729 0:cee310e0c668 34 //Ticker for algo interrupt
prathamesh1729 0:cee310e0c668 35 Ticker for_algo;
prathamesh1729 0:cee310e0c668 36
prathamesh1729 0:cee310e0c668 37 //algo function, PID funtion
prathamesh1729 0:cee310e0c668 38 void ediff_func();
prathamesh1729 0:cee310e0c668 39 void pid(void);
prathamesh1729 0:cee310e0c668 40
prathamesh1729 0:cee310e0c668 41 //delta_Left, delta_Right, turnradius and wratio_desired calculated realtime from delta (delta_Left and delta_Right are Left and Right tyre angles)
prathamesh1729 0:cee310e0c668 42 float delta_Left; //Left wheel Steer angle
prathamesh1729 0:cee310e0c668 43 float delta_Right; //Right wheel Steer angle
prathamesh1729 0:cee310e0c668 44 float turnradius;
prathamesh1729 0:cee310e0c668 45 float wratio_desired; //Right/Left
prathamesh1729 0:cee310e0c668 46
prathamesh1729 0:cee310e0c668 47 //PID constants
prathamesh1729 0:cee310e0c668 48 float kp=0;
prathamesh1729 0:cee310e0c668 49 float kd=0;
prathamesh1729 0:cee310e0c668 50 float ki=0;
prathamesh1729 0:cee310e0c668 51 float c=0;
prathamesh1729 0:cee310e0c668 52
prathamesh1729 0:cee310e0c668 53 //For PID
prathamesh1729 0:cee310e0c668 54 const float timeint = 0.1;
prathamesh1729 0:cee310e0c668 55 float integral;
prathamesh1729 0:cee310e0c668 56 float derivative;
prathamesh1729 0:cee310e0c668 57 float error_prev = 0; //error at t0
prathamesh1729 0:cee310e0c668 58 float error_new; //error at t
prathamesh1729 0:cee310e0c668 59 float wratio_actual;
prathamesh1729 0:cee310e0c668 60 float w_ratio; //ratio of controller voltages - Right/Left
prathamesh1729 0:cee310e0c668 61
prathamesh1729 0:cee310e0c668 62 // - - - For RPM Measurement - - - //
prathamesh1729 0:cee310e0c668 63
prathamesh1729 0:cee310e0c668 64 //Left and Right rear RPMs feedback-input
prathamesh1729 0:cee310e0c668 65 float rpm_Left;
prathamesh1729 0:cee310e0c668 66 float rpm_Right;
prathamesh1729 0:cee310e0c668 67 float rpm_FrontRight;
prathamesh1729 0:cee310e0c668 68 float rpm_FrontLeft;
prathamesh1729 0:cee310e0c668 69
prathamesh1729 0:cee310e0c668 70 //////////////////////////////////////CAN DATA TO DAQ////////////////////////////////////////////
prathamesh1729 0:cee310e0c668 71 CAN CANtoDAQ(p9, p10); //Check PIN Numbers;
prathamesh1729 0:cee310e0c668 72 Ticker SendDataToDAQ;
prathamesh1729 0:cee310e0c668 73 char CANdataSet1[8];
prathamesh1729 0:cee310e0c668 74 char CANdataSet2[8];
prathamesh1729 3:68d91895d991 75 char CANdataSet3[8];
prathamesh1729 3:68d91895d991 76 uint8_t FMEAHighByte = 0x00;
prathamesh1729 3:68d91895d991 77 uint8_t FMEALowByte = 0x00;
prathamesh1729 0:cee310e0c668 78
prathamesh1729 2:7e4930f48021 79 // add motor temperatures and motor currents
prathamesh1729 0:cee310e0c668 80 void PopulateData(void)
prathamesh1729 0:cee310e0c668 81 {
prathamesh1729 0:cee310e0c668 82 CANdataSet1[0] = (char)((steering + 110) /220 * 255);
prathamesh1729 5:ee37889edcab 83 CANdataSet1[1] = (char)((throttle)/5 * 255);
prathamesh1729 0:cee310e0c668 84 CANdataSet1[2] = (char)(((uint16_t)wratio_actual) >> 8); //Check range later.
prathamesh1729 0:cee310e0c668 85 CANdataSet1[3] = (char)(((uint16_t)wratio_actual)); //For now assumed to be 16 bit 0 - 65535
prathamesh1729 0:cee310e0c668 86 CANdataSet1[4] = (char)((wratio_desired - 0.5) / 1.4);
prathamesh1729 0:cee310e0c668 87 CANdataSet1[5] = (char)(throttle_Left/5 * 255);
prathamesh1729 5:ee37889edcab 88 CANdataSet1[6] = (char)((throttle_Right)/5 * 255);
prathamesh1729 0:cee310e0c668 89 CANdataSet1[7] = 0;
prathamesh1729 0:cee310e0c668 90
prathamesh1729 0:cee310e0c668 91 CANdataSet2[0] = (char)((steering2 + 110) /220 * 255);
prathamesh1729 5:ee37889edcab 92 CANdataSet2[1] = (char)((throttle2)/5 * 255);
prathamesh1729 0:cee310e0c668 93 CANdataSet2[2] = (char)(rpm_Left/1000 * 255);
prathamesh1729 0:cee310e0c668 94 CANdataSet2[3] = (char)(rpm_Right/1000 * 255);
prathamesh1729 0:cee310e0c668 95 CANdataSet2[4] = (char)(rpm_FrontLeft/1000 * 255);
prathamesh1729 0:cee310e0c668 96 CANdataSet2[5] = (char)(rpm_FrontRight/1000 * 255);
prathamesh1729 0:cee310e0c668 97 CANdataSet2[6] = (char)(yaw_rate/5 * 255); //Decide range later. For now 0 to 5.
prathamesh1729 0:cee310e0c668 98 CANdataSet2[7] = 0;
prathamesh1729 3:68d91895d991 99
prathamesh1729 3:68d91895d991 100 CANdataSet3[0] = (char)(currentLeftMux>>8);
prathamesh1729 3:68d91895d991 101 CANdataSet3[1] = (char)(currentRightMux>>8);
prathamesh1729 3:68d91895d991 102 CANdataSet3[2] = (char)(tempLeftMux>>8);
prathamesh1729 3:68d91895d991 103 CANdataSet3[3] = (char)(tempRightMux>>8);
prathamesh1729 3:68d91895d991 104 CANdataSet3[4] = (char)(FMEAHighByte);//FMEA HIGH BYTE
prathamesh1729 3:68d91895d991 105 CANdataSet3[5] = (char)(FMEALowByte);//FMEA LOW BYTE
prathamesh1729 3:68d91895d991 106 CANdataSet3[6] = (char)(yaw_rate/5 * 255); //Decide range later. For now 0 to 5.
prathamesh1729 3:68d91895d991 107 CANdataSet3[7] = 0;
prathamesh1729 0:cee310e0c668 108 }
prathamesh1729 0:cee310e0c668 109
prathamesh1729 0:cee310e0c668 110 void SendData(void)
prathamesh1729 0:cee310e0c668 111 {
prathamesh1729 0:cee310e0c668 112 PopulateData();
prathamesh1729 0:cee310e0c668 113 CANtoDAQ.write(CANMessage(1000, CANdataSet1, 8));
prathamesh1729 0:cee310e0c668 114 CANtoDAQ.write(CANMessage(1001, CANdataSet2, 8));
prathamesh1729 3:68d91895d991 115 CANtoDAQ.write(CANMessage(1002, CANdataSet3, 8));
prathamesh1729 0:cee310e0c668 116 }
prathamesh1729 0:cee310e0c668 117
prathamesh1729 0:cee310e0c668 118 ///////////////////////////////RECEIVE SPI DATA FROM ICAP//////////////////////////////////////////
prathamesh1729 0:cee310e0c668 119 SPI spi(p5,p6,p7);
prathamesh1729 0:cee310e0c668 120 DigitalOut cs(p8);
prathamesh1729 0:cee310e0c668 121 #define SPI_BYTE_ARRAY_SIZE 13
prathamesh1729 0:cee310e0c668 122 uint8_t SPIDataFromICAP[SPI_BYTE_ARRAY_SIZE];
prathamesh1729 0:cee310e0c668 123 uint8_t startByte, endByte;
prathamesh1729 5:ee37889edcab 124 uint8_t volatile flagsFromICAP;
prathamesh1729 0:cee310e0c668 125 int SPIByteCounter = 0;
prathamesh1729 0:cee310e0c668 126 bool SPIError = false;
prathamesh1729 0:cee310e0c668 127 Ticker PullDataFromICAP;
prathamesh1729 0:cee310e0c668 128
prathamesh1729 0:cee310e0c668 129 void UpdateData()
prathamesh1729 0:cee310e0c668 130 {
prathamesh1729 0:cee310e0c668 131 startByte = SPIDataFromICAP[0];
prathamesh1729 0:cee310e0c668 132 endByte = SPIDataFromICAP[12];
prathamesh1729 0:cee310e0c668 133 if (((char)startByte != '@') || ((char)endByte != '#'))
prathamesh1729 0:cee310e0c668 134 {
prathamesh1729 0:cee310e0c668 135 //Some Problem with SPI.
prathamesh1729 3:68d91895d991 136 shutdown = true;
prathamesh1729 0:cee310e0c668 137 SPIError = true;
prathamesh1729 3:68d91895d991 138 FMEALowByte|= 0x20; //0b0010 0000
prathamesh1729 3:68d91895d991 139 FMEAHighByte|= 0x04; //0b0000 0100
prathamesh1729 0:cee310e0c668 140 }
prathamesh1729 0:cee310e0c668 141
prathamesh1729 3:68d91895d991 142 steering = -110 + SPIDataFromICAP[1]*220/255;
prathamesh1729 3:68d91895d991 143 steering2 = -110 + SPIDataFromICAP[2]*220/255;
prathamesh1729 5:ee37889edcab 144 throttle = ((float)(SPIDataFromICAP[3])/255*4+0.5);
prathamesh1729 5:ee37889edcab 145 throttle2 = ((float)(SPIDataFromICAP[4])*4/255+0.5);
prathamesh1729 3:68d91895d991 146 brake = SPIDataFromICAP[5]; //Abhi ke liye let be be 0 to 255 only
prathamesh1729 3:68d91895d991 147 brake2 = SPIDataFromICAP[6]; //Abhi ke liye let be be 0 to 255 only
prathamesh1729 3:68d91895d991 148 rpm_Left = SPIDataFromICAP[7]*1000/255;
prathamesh1729 3:68d91895d991 149 rpm_Right = SPIDataFromICAP[8]*1000/255;
prathamesh1729 3:68d91895d991 150 rpm_FrontLeft = SPIDataFromICAP[9]*1000/255;
prathamesh1729 3:68d91895d991 151 rpm_FrontRight = SPIDataFromICAP[10]*1000/255;
prathamesh1729 3:68d91895d991 152 flagsFromICAP = SPIDataFromICAP[11];
prathamesh1729 5:ee37889edcab 153 //printf("Steering = %f \n", steering);
prathamesh1729 5:ee37889edcab 154 //printf("Throttle 1, 2 = %f \t %f \n", throttle, throttle2);
prathamesh1729 5:ee37889edcab 155 //printf("Throttle 1, 2 = %f \t %f \n", throttle, throttle2);
prathamesh1729 0:cee310e0c668 156 }
prathamesh1729 0:cee310e0c668 157
prathamesh1729 0:cee310e0c668 158 void SPIPullData()
prathamesh1729 0:cee310e0c668 159 {
prathamesh1729 0:cee310e0c668 160 SPIByteCounter = 0;
prathamesh1729 0:cee310e0c668 161 while(SPIByteCounter < SPI_BYTE_ARRAY_SIZE)
prathamesh1729 0:cee310e0c668 162 {
prathamesh1729 0:cee310e0c668 163 cs=0;
prathamesh1729 0:cee310e0c668 164 spi.write(0xFF);
prathamesh1729 5:ee37889edcab 165 SPIDataFromICAP[SPIByteCounter] = (uint8_t)spi.write(0xAA);
prathamesh1729 0:cee310e0c668 166 //pc.printf("%u",SPIDataFromICAP[SPIByteCounter]);
prathamesh1729 0:cee310e0c668 167 SPIByteCounter++;
prathamesh1729 0:cee310e0c668 168 cs = 1;
prathamesh1729 5:ee37889edcab 169 wait(0.001);
prathamesh1729 0:cee310e0c668 170 }
prathamesh1729 0:cee310e0c668 171 UpdateData();
prathamesh1729 0:cee310e0c668 172 }
prathamesh1729 0:cee310e0c668 173
prathamesh1729 2:7e4930f48021 174 ///////////////////////////////////////MUX///////////////////////////////////
prathamesh1729 1:c0a9790eed5b 175 unsigned short currentLeftMux;
prathamesh1729 1:c0a9790eed5b 176 unsigned short currentRightMux;
prathamesh1729 1:c0a9790eed5b 177 unsigned short tempLeftMux;
prathamesh1729 1:c0a9790eed5b 178 unsigned short tempRightMux;
prathamesh1729 2:7e4930f48021 179 //unsigned short brakeLeftMux;
prathamesh1729 2:7e4930f48021 180 //unsigned short brakeRightMux;
prathamesh1729 1:c0a9790eed5b 181 unsigned short throttleLeftMux;
prathamesh1729 1:c0a9790eed5b 182 unsigned short throttleRightMux;
prathamesh1729 1:c0a9790eed5b 183
prathamesh1729 2:7e4930f48021 184 int Mux_switch;
prathamesh1729 2:7e4930f48021 185 Ticker for_Mux;
prathamesh1729 2:7e4930f48021 186
prathamesh1729 2:7e4930f48021 187 void MuxCall()
prathamesh1729 2:7e4930f48021 188 {
prathamesh1729 2:7e4930f48021 189 switch (Mux_switch)
prathamesh1729 2:7e4930f48021 190 {
prathamesh1729 2:7e4930f48021 191 case 0:
prathamesh1729 2:7e4930f48021 192 readFromMux(throttleLeftMuxSelect, throttleLeftMux);
prathamesh1729 2:7e4930f48021 193 break;
prathamesh1729 2:7e4930f48021 194 case 1:
prathamesh1729 2:7e4930f48021 195 readFromMux(throttleRightMuxSelect, throttleRightMux);
prathamesh1729 2:7e4930f48021 196 break;
prathamesh1729 2:7e4930f48021 197 case 2:
prathamesh1729 2:7e4930f48021 198 readFromMux(tempLeftMuxSelect, tempLeftMux);
prathamesh1729 2:7e4930f48021 199 break;
prathamesh1729 2:7e4930f48021 200 case 3:
prathamesh1729 2:7e4930f48021 201 readFromMux(tempRightMuxSelect, tempRightMux);
prathamesh1729 2:7e4930f48021 202 break;
prathamesh1729 2:7e4930f48021 203 case 4:
prathamesh1729 2:7e4930f48021 204 readFromMux(currentLeftMuxSelect, currentLeftMux);
prathamesh1729 2:7e4930f48021 205 break;
prathamesh1729 2:7e4930f48021 206 case 5:
prathamesh1729 2:7e4930f48021 207 readFromMux(currentRightMuxSelect, currentRightMux);
prathamesh1729 2:7e4930f48021 208 break;
prathamesh1729 2:7e4930f48021 209 }
prathamesh1729 2:7e4930f48021 210 Mux_switch++;
prathamesh1729 2:7e4930f48021 211 if (Mux_switch == 6)
prathamesh1729 2:7e4930f48021 212 {
prathamesh1729 2:7e4930f48021 213 Mux_switch = 0;
prathamesh1729 2:7e4930f48021 214 }
prathamesh1729 2:7e4930f48021 215 }
prathamesh1729 2:7e4930f48021 216
prathamesh1729 2:7e4930f48021 217 //////////////////////////////////DAC/////////////////////////////////////////
prathamesh1729 1:c0a9790eed5b 218 Ticker for_DAC;
prathamesh1729 1:c0a9790eed5b 219 int DAC_switch;
prathamesh1729 1:c0a9790eed5b 220
prathamesh1729 2:7e4930f48021 221 void outputToDAC()
prathamesh1729 2:7e4930f48021 222 {
prathamesh1729 1:c0a9790eed5b 223 switch (DAC_switch)
prathamesh1729 1:c0a9790eed5b 224 {
prathamesh1729 1:c0a9790eed5b 225 case 0:
prathamesh1729 1:c0a9790eed5b 226 writeToDAC(throttleLeftDACChannel, (unsigned short) (throttle_Left * 65535/5) , WRITE_TO_INPUT_REGISTER_AND_UPDATE_DAC_REGISTER);
prathamesh1729 1:c0a9790eed5b 227 break;
prathamesh1729 1:c0a9790eed5b 228 case 1:
prathamesh1729 1:c0a9790eed5b 229 writeToDAC(throttleRightDACChannel, (unsigned short) (throttle_Right * 65535/5) , WRITE_TO_INPUT_REGISTER_AND_UPDATE_DAC_REGISTER);
prathamesh1729 1:c0a9790eed5b 230 break;
prathamesh1729 1:c0a9790eed5b 231 }
prathamesh1729 1:c0a9790eed5b 232 DAC_switch++;
prathamesh1729 1:c0a9790eed5b 233 if (DAC_switch == 2)
prathamesh1729 1:c0a9790eed5b 234 {
prathamesh1729 1:c0a9790eed5b 235 DAC_switch = 0;
prathamesh1729 1:c0a9790eed5b 236 }
prathamesh1729 1:c0a9790eed5b 237 }
prathamesh1729 1:c0a9790eed5b 238
prathamesh1729 3:68d91895d991 239
prathamesh1729 3:68d91895d991 240 // - - - - FMEA OUTPUT - - - - //
prathamesh1729 3:68d91895d991 241
prathamesh1729 3:68d91895d991 242 //Openloop Flag
prathamesh1729 5:ee37889edcab 243 ///////////////////Made FALSE deliberately for OFFLINE Testing//////////////////////
prathamesh1729 5:ee37889edcab 244 ///////////////////Should ideally be true at startup as the car is at standstill.///
prathamesh1729 5:ee37889edcab 245 volatile bool openloop_lowrpm = false;
prathamesh1729 3:68d91895d991 246 volatile bool openloop_driver = false;
prathamesh1729 3:68d91895d991 247 const float rpm_openloop_limit = 100;
prathamesh1729 3:68d91895d991 248
prathamesh1729 0:cee310e0c668 249
prathamesh1729 3:68d91895d991 250 bool Dash = false;
prathamesh1729 3:68d91895d991 251 bool throttleLeftDACFailure = false;
prathamesh1729 3:68d91895d991 252 bool throttleRightDACFailure = false;
prathamesh1729 3:68d91895d991 253 bool throttleComparisonFailure = false;
prathamesh1729 3:68d91895d991 254 bool steeringComparisonFailure = false;
prathamesh1729 3:68d91895d991 255 bool useBoschSteeringRequest = false;
prathamesh1729 3:68d91895d991 256 bool useBoschSteeringDriverApproval = false;
prathamesh1729 3:68d91895d991 257 bool shutdown = false;
prathamesh1729 3:68d91895d991 258 bool throttleLeftlimitcross = false;
prathamesh1729 3:68d91895d991 259 bool throttleRightlimitcross = false;
prathamesh1729 0:cee310e0c668 260
prathamesh1729 3:68d91895d991 261 volatile int fmea_switch = 0;
prathamesh1729 3:68d91895d991 262 Ticker for_FmeaCall;
prathamesh1729 0:cee310e0c668 263
prathamesh1729 0:cee310e0c668 264 void FmeaCall()
prathamesh1729 0:cee310e0c668 265 {
prathamesh1729 0:cee310e0c668 266 switch (fmea_switch)
prathamesh1729 0:cee310e0c668 267 {
prathamesh1729 2:7e4930f48021 268 case 0:
prathamesh1729 3:68d91895d991 269 throttle_Left_limitcross_fmea();
prathamesh1729 0:cee310e0c668 270 break;
prathamesh1729 2:7e4930f48021 271 case 1:
prathamesh1729 3:68d91895d991 272 throttle_Right_limitcross_fmea();
prathamesh1729 2:7e4930f48021 273 break;
prathamesh1729 2:7e4930f48021 274 case 2:
prathamesh1729 2:7e4930f48021 275 throttle_comparison_fmea();
prathamesh1729 0:cee310e0c668 276 break;
prathamesh1729 2:7e4930f48021 277 case 3:
prathamesh1729 4:66b2a94a607f 278 steering_comparison_fmea();
prathamesh1729 0:cee310e0c668 279 break;
prathamesh1729 2:7e4930f48021 280 case 4:
prathamesh1729 2:7e4930f48021 281 mux_feedback_fmea();
prathamesh1729 0:cee310e0c668 282 break;
prathamesh1729 0:cee310e0c668 283 }
prathamesh1729 0:cee310e0c668 284 fmea_switch++;
prathamesh1729 4:66b2a94a607f 285 if (fmea_switch == 5)
prathamesh1729 0:cee310e0c668 286 {
prathamesh1729 0:cee310e0c668 287 fmea_switch = 0;
prathamesh1729 0:cee310e0c668 288 }
prathamesh1729 0:cee310e0c668 289 }
prathamesh1729 3:68d91895d991 290
prathamesh1729 3:68d91895d991 291 ////////////////////////////////////////////////////MAIN////////////////////////////////////////////////////
prathamesh1729 3:68d91895d991 292 int main()
prathamesh1729 3:68d91895d991 293 {
prathamesh1729 5:ee37889edcab 294 //for_FmeaCall.attach(&FmeaCall,1);
prathamesh1729 3:68d91895d991 295 for_algo.attach_us(&ediff_func,(algoperiod)); //call ediff func every "algoperiod" sec
prathamesh1729 3:68d91895d991 296 SendDataToDAQ.attach(&SendData, 0.05);
prathamesh1729 3:68d91895d991 297
prathamesh1729 3:68d91895d991 298 //DAC in Main
prathamesh1729 3:68d91895d991 299 initDAC();
prathamesh1729 3:68d91895d991 300 for_DAC.attach(&outputToDAC, 0.01);
prathamesh1729 3:68d91895d991 301
prathamesh1729 3:68d91895d991 302 //MUX in Main
prathamesh1729 3:68d91895d991 303 for_Mux.attach(&MuxCall, 0.5);
prathamesh1729 3:68d91895d991 304
prathamesh1729 3:68d91895d991 305 cs = 1;
prathamesh1729 3:68d91895d991 306 spi.format(8,3);
prathamesh1729 3:68d91895d991 307 spi.frequency(1000000);
prathamesh1729 3:68d91895d991 308 PullDataFromICAP.attach(&SPIPullData, 0.02);
prathamesh1729 3:68d91895d991 309 spi.write(0xAA);
prathamesh1729 3:68d91895d991 310
prathamesh1729 3:68d91895d991 311 while(1)
prathamesh1729 3:68d91895d991 312 {
prathamesh1729 4:66b2a94a607f 313 //openloop_driver is set & corresponding bit(s) from flagsFromICAP is reset if driver gives approval to dash message request for openloop
prathamesh1729 3:68d91895d991 314
prathamesh1729 4:66b2a94a607f 315 // useBoschSteeringDriverApproval is set & useBoschSteeringRequest is reset if driver gives approval to dash message request for using Bosch Steering
prathamesh1729 3:68d91895d991 316
prathamesh1729 4:66b2a94a607f 317 if ((flagsFromICAP & 0x78) && !openloop_driver) //0b 0111 1000
prathamesh1729 3:68d91895d991 318 {
prathamesh1729 4:66b2a94a607f 319 //send message to DASH that asks driver permission for open loop
prathamesh1729 3:68d91895d991 320 }
prathamesh1729 4:66b2a94a607f 321 if (useBoschSteeringRequest)
prathamesh1729 4:66b2a94a607f 322 {
prathamesh1729 4:66b2a94a607f 323 //send message to DASH that asks driver permission for using Bosch Steering
prathamesh1729 5:ee37889edcab 324 }
prathamesh1729 5:ee37889edcab 325
prathamesh1729 3:68d91895d991 326 }
prathamesh1729 3:68d91895d991 327 }
prathamesh1729 3:68d91895d991 328
prathamesh1729 3:68d91895d991 329 //Function reads stearing and throttle input, calls PID Function and gives Throttle Left and Right PWM outputs
prathamesh1729 3:68d91895d991 330 void ediff_func()
prathamesh1729 3:68d91895d991 331 {
prathamesh1729 3:68d91895d991 332 //call pid function which gives v1, v2 output
prathamesh1729 3:68d91895d991 333 pid();
prathamesh1729 3:68d91895d991 334
prathamesh1729 3:68d91895d991 335 //send throttle_Left and throttle_Right to Kelly Controllers
prathamesh1729 3:68d91895d991 336 }