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@0:cee310e0c668, 2013-03-07 (annotated)

Committer:
prathamesh1729
Date:
Thu Mar 07 18:22:38 2013 +0000
Revision:
0:cee310e0c668
Child:
1:c0a9790eed5b
CAN with DAQ, SPI from ICAP updated.

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 0:cee310e0c668 3
prathamesh1729 0:cee310e0c668 4 // - - - Parameters - - - //
prathamesh1729 0:cee310e0c668 5
prathamesh1729 0:cee310e0c668 6 //car parameters
prathamesh1729 0:cee310e0c668 7 const float trackwidth = 49*0.0254;
prathamesh1729 0:cee310e0c668 8 const float wheelradius = 0.254;
prathamesh1729 0:cee310e0c668 9 const float rear_wheelbase = 0.686816; //rear weightbias*wheelbase
prathamesh1729 0:cee310e0c668 10
prathamesh1729 0:cee310e0c668 11 //constant parameters
prathamesh1729 0:cee310e0c668 12 const float full_throttle=5;
prathamesh1729 0:cee310e0c668 13 const float dead_steering=7;
prathamesh1729 0:cee310e0c668 14 const float integral_saturation=0.5;
prathamesh1729 0:cee310e0c668 15 const float dead_rpm = 10;
prathamesh1729 0:cee310e0c668 16
prathamesh1729 0:cee310e0c668 17 // - - - Inputs - - - //
prathamesh1729 0:cee310e0c668 18
prathamesh1729 0:cee310e0c668 19 //steering and throttle inputs
prathamesh1729 0:cee310e0c668 20 float steering,steering2; //input steering wheel angle in degrees
prathamesh1729 0:cee310e0c668 21 float throttle, throttle2; //throttle input from pedal
prathamesh1729 0:cee310e0c668 22 float yaw_rate;
prathamesh1729 0:cee310e0c668 23
prathamesh1729 0:cee310e0c668 24 // - - - Outputs - - - //
prathamesh1729 0:cee310e0c668 25
prathamesh1729 0:cee310e0c668 26 // throttle output values
prathamesh1729 0:cee310e0c668 27 float throttle_Left, throttle_Right;
prathamesh1729 0:cee310e0c668 28
prathamesh1729 0:cee310e0c668 29 // - - - For Basic Algo and Steering to turnradius function - - - //
prathamesh1729 0:cee310e0c668 30
prathamesh1729 0:cee310e0c668 31 //Ticker for algo interrupt
prathamesh1729 0:cee310e0c668 32 Ticker for_algo;
prathamesh1729 0:cee310e0c668 33
prathamesh1729 0:cee310e0c668 34 //algo function, PID funtion
prathamesh1729 0:cee310e0c668 35 void ediff_func();
prathamesh1729 0:cee310e0c668 36 void pid(void);
prathamesh1729 0:cee310e0c668 37
prathamesh1729 0:cee310e0c668 38 //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 39 float delta_Left; //Left wheel Steer angle
prathamesh1729 0:cee310e0c668 40 float delta_Right; //Right wheel Steer angle
prathamesh1729 0:cee310e0c668 41 float turnradius;
prathamesh1729 0:cee310e0c668 42 float wratio_desired; //Right/Left
prathamesh1729 0:cee310e0c668 43
prathamesh1729 0:cee310e0c668 44 //Openloop Flag
prathamesh1729 0:cee310e0c668 45 volatile bool openloop_lowrpm = true;
prathamesh1729 0:cee310e0c668 46 volatile bool openloop_driver = false;
prathamesh1729 0:cee310e0c668 47 const float rpm_openloop_limit = 100;
prathamesh1729 0:cee310e0c668 48
prathamesh1729 0:cee310e0c668 49 //PID constants
prathamesh1729 0:cee310e0c668 50 float kp=0;
prathamesh1729 0:cee310e0c668 51 float kd=0;
prathamesh1729 0:cee310e0c668 52 float ki=0;
prathamesh1729 0:cee310e0c668 53 float c=0;
prathamesh1729 0:cee310e0c668 54
prathamesh1729 0:cee310e0c668 55 //For PID
prathamesh1729 0:cee310e0c668 56 const float timeint = 0.1;
prathamesh1729 0:cee310e0c668 57 float integral;
prathamesh1729 0:cee310e0c668 58 float derivative;
prathamesh1729 0:cee310e0c668 59 float error_prev = 0; //error at t0
prathamesh1729 0:cee310e0c668 60 float error_new; //error at t
prathamesh1729 0:cee310e0c668 61 float wratio_actual;
prathamesh1729 0:cee310e0c668 62 float w_ratio; //ratio of controller voltages - Right/Left
prathamesh1729 0:cee310e0c668 63
prathamesh1729 0:cee310e0c668 64 // - - - For RPM Measurement - - - //
prathamesh1729 0:cee310e0c668 65
prathamesh1729 0:cee310e0c668 66 //Left and Right rear RPMs feedback-input
prathamesh1729 0:cee310e0c668 67 float rpm_Left;
prathamesh1729 0:cee310e0c668 68 float rpm_Right;
prathamesh1729 0:cee310e0c668 69 float rpm_FrontRight;
prathamesh1729 0:cee310e0c668 70 float rpm_FrontLeft;
prathamesh1729 0:cee310e0c668 71
prathamesh1729 0:cee310e0c668 72 // - - - - FMEA OUTPUT - - - - //
prathamesh1729 0:cee310e0c668 73 bool shutdown = false;
prathamesh1729 0:cee310e0c668 74 volatile int fmea_switch = 0;
prathamesh1729 0:cee310e0c668 75 Ticker for_FmeaCall;
prathamesh1729 0:cee310e0c668 76 void FmeaCall();
prathamesh1729 0:cee310e0c668 77
prathamesh1729 0:cee310e0c668 78 //////////////////////////////////////CAN DATA TO DAQ////////////////////////////////////////////
prathamesh1729 0:cee310e0c668 79 CAN CANtoDAQ(p9, p10); //Check PIN Numbers;
prathamesh1729 0:cee310e0c668 80 Ticker SendDataToDAQ;
prathamesh1729 0:cee310e0c668 81 char CANdataSet1[8];
prathamesh1729 0:cee310e0c668 82 char CANdataSet2[8];
prathamesh1729 0:cee310e0c668 83
prathamesh1729 0:cee310e0c668 84 void PopulateData(void)
prathamesh1729 0:cee310e0c668 85 {
prathamesh1729 0:cee310e0c668 86 CANdataSet1[0] = (char)((steering + 110) /220 * 255);
prathamesh1729 0:cee310e0c668 87 CANdataSet1[1] = (char)(throttle/5 * 255);
prathamesh1729 0:cee310e0c668 88 CANdataSet1[2] = (char)(((uint16_t)wratio_actual) >> 8); //Check range later.
prathamesh1729 0:cee310e0c668 89 CANdataSet1[3] = (char)(((uint16_t)wratio_actual)); //For now assumed to be 16 bit 0 - 65535
prathamesh1729 0:cee310e0c668 90 CANdataSet1[4] = (char)((wratio_desired - 0.5) / 1.4);
prathamesh1729 0:cee310e0c668 91 CANdataSet1[5] = (char)(throttle_Left/5 * 255);
prathamesh1729 0:cee310e0c668 92 CANdataSet1[6] = (char)(throttle_Right/5 * 255);
prathamesh1729 0:cee310e0c668 93 CANdataSet1[7] = 0;
prathamesh1729 0:cee310e0c668 94
prathamesh1729 0:cee310e0c668 95 CANdataSet2[0] = (char)((steering2 + 110) /220 * 255);
prathamesh1729 0:cee310e0c668 96 CANdataSet2[1] = (char)(throttle2/5 * 255);
prathamesh1729 0:cee310e0c668 97 CANdataSet2[2] = (char)(rpm_Left/1000 * 255);
prathamesh1729 0:cee310e0c668 98 CANdataSet2[3] = (char)(rpm_Right/1000 * 255);
prathamesh1729 0:cee310e0c668 99 CANdataSet2[4] = (char)(rpm_FrontLeft/1000 * 255);
prathamesh1729 0:cee310e0c668 100 CANdataSet2[5] = (char)(rpm_FrontRight/1000 * 255);
prathamesh1729 0:cee310e0c668 101 CANdataSet2[6] = (char)(yaw_rate/5 * 255); //Decide range later. For now 0 to 5.
prathamesh1729 0:cee310e0c668 102 CANdataSet2[7] = 0;
prathamesh1729 0:cee310e0c668 103 }
prathamesh1729 0:cee310e0c668 104
prathamesh1729 0:cee310e0c668 105 void SendData(void)
prathamesh1729 0:cee310e0c668 106 {
prathamesh1729 0:cee310e0c668 107 PopulateData();
prathamesh1729 0:cee310e0c668 108 CANtoDAQ.write(CANMessage(1000, CANdataSet1, 8));
prathamesh1729 0:cee310e0c668 109 CANtoDAQ.write(CANMessage(1001, CANdataSet2, 8));
prathamesh1729 0:cee310e0c668 110 }
prathamesh1729 0:cee310e0c668 111
prathamesh1729 0:cee310e0c668 112 ///////////////////////////////RECEIVE SPI DATA FROM ICAP//////////////////////////////////////////
prathamesh1729 0:cee310e0c668 113 SPI spi(p5,p6,p7);
prathamesh1729 0:cee310e0c668 114 DigitalOut cs(p8);
prathamesh1729 0:cee310e0c668 115 #define SPI_BYTE_ARRAY_SIZE 13
prathamesh1729 0:cee310e0c668 116 uint8_t SPIDataFromICAP[SPI_BYTE_ARRAY_SIZE];
prathamesh1729 0:cee310e0c668 117 uint8_t startByte, endByte;
prathamesh1729 0:cee310e0c668 118 int SPIByteCounter = 0;
prathamesh1729 0:cee310e0c668 119 bool SPIError = false;
prathamesh1729 0:cee310e0c668 120 Ticker PullDataFromICAP;
prathamesh1729 0:cee310e0c668 121
prathamesh1729 0:cee310e0c668 122 void UpdateData()
prathamesh1729 0:cee310e0c668 123 {
prathamesh1729 0:cee310e0c668 124 startByte = SPIDataFromICAP[0];
prathamesh1729 0:cee310e0c668 125 endByte = SPIDataFromICAP[12];
prathamesh1729 0:cee310e0c668 126 if (((char)startByte != '@') || ((char)endByte != '#'))
prathamesh1729 0:cee310e0c668 127 {
prathamesh1729 0:cee310e0c668 128 //Some Problem with SPI.
prathamesh1729 0:cee310e0c668 129 SPIError = true;
prathamesh1729 0:cee310e0c668 130 }
prathamesh1729 0:cee310e0c668 131
prathamesh1729 0:cee310e0c668 132 /*
prathamesh1729 0:cee310e0c668 133 steering = SPIDataFromICAP[1];
prathamesh1729 0:cee310e0c668 134 steering2 = SPIDataFromICAP[2];
prathamesh1729 0:cee310e0c668 135 throttle = SPIDataFromICAP[3];
prathamesh1729 0:cee310e0c668 136 throttle2 = SPIDataFromICAP[4];
prathamesh1729 0:cee310e0c668 137 brake = SPIDataFromICAP[5];
prathamesh1729 0:cee310e0c668 138 brake2 = SPIDataFromICAP[6];
prathamesh1729 0:cee310e0c668 139 rpm_Left = SPIDataFromICAP[7];
prathamesh1729 0:cee310e0c668 140 rpm_Right = SPIDataFromICAP[8];
prathamesh1729 0:cee310e0c668 141 rpm_FrontLeft = SPIDataFromICAP[9];
prathamesh1729 0:cee310e0c668 142 rpm_FrontRight = SPIDataFromICAP[10];
prathamesh1729 0:cee310e0c668 143 flagsFromICAP = SPIDataFromICAP[11];
prathamesh1729 0:cee310e0c668 144 */
prathamesh1729 0:cee310e0c668 145 }
prathamesh1729 0:cee310e0c668 146
prathamesh1729 0:cee310e0c668 147 void SPIPullData()
prathamesh1729 0:cee310e0c668 148 {
prathamesh1729 0:cee310e0c668 149 SPIByteCounter = 0;
prathamesh1729 0:cee310e0c668 150 while(SPIByteCounter < SPI_BYTE_ARRAY_SIZE)
prathamesh1729 0:cee310e0c668 151 {
prathamesh1729 0:cee310e0c668 152 cs=0;
prathamesh1729 0:cee310e0c668 153 spi.write(0xFF);
prathamesh1729 0:cee310e0c668 154 SPIDataFromICAP[SPIByteCounter] = spi.write(0xAA);
prathamesh1729 0:cee310e0c668 155 //pc.printf("%u",SPIDataFromICAP[SPIByteCounter]);
prathamesh1729 0:cee310e0c668 156 SPIByteCounter++;
prathamesh1729 0:cee310e0c668 157 cs = 1;
prathamesh1729 0:cee310e0c668 158 wait(0.0001);
prathamesh1729 0:cee310e0c668 159 }
prathamesh1729 0:cee310e0c668 160 UpdateData();
prathamesh1729 0:cee310e0c668 161 }
prathamesh1729 0:cee310e0c668 162
prathamesh1729 0:cee310e0c668 163 ////////////////////////////////////////////////////MAIN////////////////////////////////////////////////////
prathamesh1729 0:cee310e0c668 164 int main()
prathamesh1729 0:cee310e0c668 165 {
prathamesh1729 0:cee310e0c668 166 for_FmeaCall.attach(&FmeaCall,1);
prathamesh1729 0:cee310e0c668 167 for_algo.attach_us(&ediff_func,(algoperiod)); //call ediff func every "algoperiod" sec
prathamesh1729 0:cee310e0c668 168 SendDataToDAQ.attach(&SendData, 0.05);
prathamesh1729 0:cee310e0c668 169
prathamesh1729 0:cee310e0c668 170 cs = 1;
prathamesh1729 0:cee310e0c668 171 spi.format(8,3);
prathamesh1729 0:cee310e0c668 172 spi.frequency(1000000);
prathamesh1729 0:cee310e0c668 173 PullDataFromICAP.attach(&SPIPullData, 0.02);
prathamesh1729 0:cee310e0c668 174 spi.write(0xAA);
prathamesh1729 0:cee310e0c668 175
prathamesh1729 0:cee310e0c668 176 while(1)
prathamesh1729 0:cee310e0c668 177 {
prathamesh1729 0:cee310e0c668 178 //Take steering 1,2 and throttle 1,2 from Input capture
prathamesh1729 0:cee310e0c668 179 }
prathamesh1729 0:cee310e0c668 180 }
prathamesh1729 0:cee310e0c668 181
prathamesh1729 0:cee310e0c668 182 //Function reads stearing and throttle input, calls PID Function and gives Throttle Left and Right PWM outputs
prathamesh1729 0:cee310e0c668 183 void ediff_func()
prathamesh1729 0:cee310e0c668 184 {
prathamesh1729 0:cee310e0c668 185 //call pid function which gives v1, v2 output
prathamesh1729 0:cee310e0c668 186 pid();
prathamesh1729 0:cee310e0c668 187
prathamesh1729 0:cee310e0c668 188 //send throttle_Left and throttle_Right to Kelly Controllers
prathamesh1729 0:cee310e0c668 189 }
prathamesh1729 0:cee310e0c668 190
prathamesh1729 0:cee310e0c668 191 void FmeaCall()
prathamesh1729 0:cee310e0c668 192 {
prathamesh1729 0:cee310e0c668 193 switch (fmea_switch)
prathamesh1729 0:cee310e0c668 194 {
prathamesh1729 0:cee310e0c668 195 case 0: throttle_Left_pulldown_fmea();
prathamesh1729 0:cee310e0c668 196 break;
prathamesh1729 0:cee310e0c668 197 case 1: throttle_Right_pulldown_fmea();
prathamesh1729 0:cee310e0c668 198 break;
prathamesh1729 0:cee310e0c668 199 case 2: throttle_comparison_fmea();
prathamesh1729 0:cee310e0c668 200 break;
prathamesh1729 0:cee310e0c668 201 case 3: steering2_pulldown_fmea();
prathamesh1729 0:cee310e0c668 202 break;
prathamesh1729 0:cee310e0c668 203 case 4: steering_comparison_fmea();
prathamesh1729 0:cee310e0c668 204 break;
prathamesh1729 0:cee310e0c668 205 }
prathamesh1729 0:cee310e0c668 206 fmea_switch++;
prathamesh1729 0:cee310e0c668 207 if (fmea_switch == 5)
prathamesh1729 0:cee310e0c668 208 {
prathamesh1729 0:cee310e0c668 209 fmea_switch = 0;
prathamesh1729 0:cee310e0c668 210 }
prathamesh1729 0:cee310e0c668 211 }