sakthi priya amirtharaj / Mbed 2 deprecated BAE_hw_test1_4

gotta check ctrl algo - it gives negative value

Dependencies:   mbed-rtos mbed

Fork of BAE_hw_test1_3 by sakthi priya amirtharaj

HK.cpp@11:6b00f3b1be17, 2015-03-04 (annotated)

Committer:
sakthipriya
Date:
Wed Mar 04 17:15:15 2015 +0000
Revision:
11:6b00f3b1be17
Parent:
5:4199c0dfed33 
control algo - torque converted to moment

Who changed what in which revision?

UserRevisionLine numberNew contents of line
raizel_varun 0:ebdf4f859dca 1 #include "HK.h"
raizel_varun 0:ebdf4f859dca 2 #include "pin_config.h"
raizel_varun 0:ebdf4f859dca 3
raizel_varun 0:ebdf4f859dca 4
raizel_varun 0:ebdf4f859dca 5 //GPIO pins used=> D2-D12, A0-A1
raizel_varun 0:ebdf4f859dca 6
greenroshks 2:edd107ea4740 7 DigitalOut SelectLinesA[]={PIN43,PIN44,PIN45,PIN46};//to mux1=>voltage mux , PTA 13-16 , CHNGE TO PIN43 LATER
greenroshks 2:edd107ea4740 8 DigitalOut SelectLinesB[]={PTD3,PTC4,PTC8,PIN59};//to mux2=>current mux(differential mux) , PTB 3,7,8,9
sakthipriya 5:4199c0dfed33 9 DigitalOut SelectLinesC[]={PIN64,PIN65,PIN66,PTA12};//to mux3=>temp mux PTB 18-21
raizel_varun 0:ebdf4f859dca 10
raizel_varun 0:ebdf4f859dca 11 //--------------------------------------------MSB is SelectLines[0],LSB is SelectLines[3]--------------------------------
raizel_varun 0:ebdf4f859dca 12
raizel_varun 0:ebdf4f859dca 13 AnalogIn CurrentInput(PIN53); // Input from Current Mux PTB0
raizel_varun 0:ebdf4f859dca 14 AnalogIn VoltageInput(PIN54); // Input from Voltage Multiplexer PTB1
raizel_varun 0:ebdf4f859dca 15 AnalogIn TemperatureInput(PIN55); /*PTB2 Input from Temperature Multiplexer,thermistor Multiplexer- same multiplexer for both(lines 1-4 for thermistor,line 0 for temperature sensor)*/
raizel_varun 0:ebdf4f859dca 16
raizel_varun 0:ebdf4f859dca 17
raizel_varun 0:ebdf4f859dca 18
raizel_varun 0:ebdf4f859dca 19
raizel_varun 0:ebdf4f859dca 20 int quantiz(float start,float step,float x) // accepts min and measured values and step->quantises on a scale 0-15..(4 bit quantisation)
raizel_varun 0:ebdf4f859dca 21 {
raizel_varun 0:ebdf4f859dca 22 int y=(x-start)/step;
raizel_varun 0:ebdf4f859dca 23 if(y<=0)y=0;
raizel_varun 0:ebdf4f859dca 24 if(y>=15)y=15;
raizel_varun 0:ebdf4f859dca 25 return y;
raizel_varun 0:ebdf4f859dca 26 }
raizel_varun 0:ebdf4f859dca 27
raizel_varun 0:ebdf4f859dca 28 void init_beacon(ShortBeacy* x,SensorDataQuantised y)
raizel_varun 0:ebdf4f859dca 29 {
raizel_varun 0:ebdf4f859dca 30 (*x).Voltage[0]=2;//quantised value
raizel_varun 0:ebdf4f859dca 31 (*x).Temp[0]=y.PanelTemperature[0];//quantised value
raizel_varun 0:ebdf4f859dca 32 (*x).Temp[1]=y.PanelTemperature[1];//quantised value
raizel_varun 0:ebdf4f859dca 33 (*x).AngularSpeed[0]=y.AngularSpeed[0];
raizel_varun 0:ebdf4f859dca 34 (*x).AngularSpeed[1]=y.AngularSpeed[1];
raizel_varun 0:ebdf4f859dca 35
raizel_varun 0:ebdf4f859dca 36 (*x).SubsystemStatus[0]=145;//dummy values----------to be changed-------------------
raizel_varun 0:ebdf4f859dca 37 (*x).ErrorFlag[0]=3;//dummy values----------to be changed-------------------
raizel_varun 0:ebdf4f859dca 38 }
raizel_varun 0:ebdf4f859dca 39
raizel_varun 0:ebdf4f859dca 40 SensorData Sensor;
raizel_varun 0:ebdf4f859dca 41 SensorDataQuantised SensorQuantised;
raizel_varun 0:ebdf4f859dca 42 ShortBeacy Shortbeacon;
raizel_varun 0:ebdf4f859dca 43 void FUNC_HK_MAIN()
raizel_varun 0:ebdf4f859dca 44 {
raizel_varun 0:ebdf4f859dca 45 //define structure variables
raizel_varun 0:ebdf4f859dca 46
raizel_varun 0:ebdf4f859dca 47
raizel_varun 0:ebdf4f859dca 48
raizel_varun 0:ebdf4f859dca 49
raizel_varun 0:ebdf4f859dca 50 //initialise all selectlines to zeroes->1st line of muxes selected
raizel_varun 0:ebdf4f859dca 51 SelectLinesA[0]=SelectLinesA[1]=SelectLinesA[2]=SelectLinesA[3]=0;
raizel_varun 0:ebdf4f859dca 52 SelectLinesB[0]=SelectLinesB[1]=SelectLinesB[2]=0;
raizel_varun 0:ebdf4f859dca 53 SelectLinesC[0]=SelectLinesC[1]=SelectLinesC[2]=SelectLinesC[3]=0;
raizel_varun 0:ebdf4f859dca 54
raizel_varun 0:ebdf4f859dca 55 int LoopIterator;
raizel_varun 0:ebdf4f859dca 56 int SelectLineIterator;
raizel_varun 0:ebdf4f859dca 57
raizel_varun 0:ebdf4f859dca 58 float resistance_thermistor,voltage_thermistor;//for thermistor
raizel_varun 0:ebdf4f859dca 59
raizel_varun 0:ebdf4f859dca 60 //measurement from voltage sensor=> 16 sensors in place
raizel_varun 0:ebdf4f859dca 61 for(LoopIterator=0; LoopIterator<16; LoopIterator++)
raizel_varun 0:ebdf4f859dca 62 {
raizel_varun 0:ebdf4f859dca 63 //following lines read the sensor values and stores them in 'SensorData' structure's variable 'Sensor'
raizel_varun 0:ebdf4f859dca 64 Sensor.Voltage[LoopIterator]=(VoltageInput.read()*3.3*5.545454);//resistors in voltage divider=>15Mohm,3.3Mohm
raizel_varun 0:ebdf4f859dca 65
raizel_varun 0:ebdf4f859dca 66 if(LoopIterator%2==0)
raizel_varun 0:ebdf4f859dca 67 SensorQuantised.Voltage[LoopIterator/2]=quantiz(vstart,vstep,Sensor.Voltage[LoopIterator]);
raizel_varun 0:ebdf4f859dca 68
raizel_varun 0:ebdf4f859dca 69 else
raizel_varun 0:ebdf4f859dca 70 SensorQuantised.Voltage[(LoopIterator)/2]=SensorQuantised.Voltage[(LoopIterator)/2]<<4+quantiz(vstart,vstep,Sensor.Voltage[LoopIterator]);
raizel_varun 0:ebdf4f859dca 71
raizel_varun 0:ebdf4f859dca 72
raizel_varun 0:ebdf4f859dca 73
raizel_varun 0:ebdf4f859dca 74 // The following lines are used to iterate the select lines from 0 to 15
raizel_varun 0:ebdf4f859dca 75 //following is an algorithm similar to counting binary numbers of 4 bit
raizel_varun 0:ebdf4f859dca 76 for(SelectLineIterator=3;SelectLineIterator>=0;SelectLineIterator--)
raizel_varun 0:ebdf4f859dca 77 {
raizel_varun 0:ebdf4f859dca 78 if(SelectLinesA[SelectLineIterator]==0)
raizel_varun 0:ebdf4f859dca 79 {
raizel_varun 0:ebdf4f859dca 80 SelectLinesA[SelectLineIterator]=1;
raizel_varun 0:ebdf4f859dca 81 break;
raizel_varun 0:ebdf4f859dca 82 }
raizel_varun 0:ebdf4f859dca 83 else SelectLinesA[SelectLineIterator]=0;
raizel_varun 0:ebdf4f859dca 84
raizel_varun 0:ebdf4f859dca 85 }
raizel_varun 0:ebdf4f859dca 86
raizel_varun 0:ebdf4f859dca 87
raizel_varun 0:ebdf4f859dca 88 wait_us(10.0); // A delay of 10 microseconds between each sensor output. Can be changed.
raizel_varun 0:ebdf4f859dca 89
raizel_varun 0:ebdf4f859dca 90 }
raizel_varun 0:ebdf4f859dca 91
raizel_varun 0:ebdf4f859dca 92
raizel_varun 0:ebdf4f859dca 93
raizel_varun 0:ebdf4f859dca 94
raizel_varun 0:ebdf4f859dca 95
raizel_varun 0:ebdf4f859dca 96 //measurement from current sensor=> 8 sensors in place
raizel_varun 0:ebdf4f859dca 97
raizel_varun 0:ebdf4f859dca 98 for(LoopIterator=0; LoopIterator<8; LoopIterator++)
raizel_varun 0:ebdf4f859dca 99 {
raizel_varun 0:ebdf4f859dca 100 //following lines read the sensor values and stores them in 'SensorData' structure variable 'Sensor'
raizel_varun 0:ebdf4f859dca 101 Sensor.Current[LoopIterator]=(CurrentInput.read()*3.3/(50*rsens));
raizel_varun 0:ebdf4f859dca 102 if(LoopIterator%2==0)
raizel_varun 0:ebdf4f859dca 103 SensorQuantised.Current[LoopIterator/2]=quantiz(cstart,cstep,Sensor.Current[LoopIterator]);
raizel_varun 0:ebdf4f859dca 104 else
raizel_varun 0:ebdf4f859dca 105 SensorQuantised.Current[(LoopIterator)/2]=SensorQuantised.Current[(LoopIterator)/2]<<4+quantiz(cstart,cstep,Sensor.Current[LoopIterator]);
raizel_varun 0:ebdf4f859dca 106
raizel_varun 0:ebdf4f859dca 107
raizel_varun 0:ebdf4f859dca 108 // The following lines are used to iterate the select lines from 0 to 7
raizel_varun 0:ebdf4f859dca 109 //following is an algorithm similar to counting binary numbers of 3 bits
raizel_varun 0:ebdf4f859dca 110 for(SelectLineIterator=2;SelectLineIterator>=0;SelectLineIterator--)
raizel_varun 0:ebdf4f859dca 111 {
raizel_varun 0:ebdf4f859dca 112 if(SelectLinesB[SelectLineIterator]==0)
raizel_varun 0:ebdf4f859dca 113 {
raizel_varun 0:ebdf4f859dca 114 SelectLinesB[SelectLineIterator]=1;
raizel_varun 0:ebdf4f859dca 115 break;
raizel_varun 0:ebdf4f859dca 116 }
raizel_varun 0:ebdf4f859dca 117 else SelectLinesB[SelectLineIterator]=0;
raizel_varun 0:ebdf4f859dca 118
raizel_varun 0:ebdf4f859dca 119 }
raizel_varun 0:ebdf4f859dca 120
raizel_varun 0:ebdf4f859dca 121
raizel_varun 0:ebdf4f859dca 122 wait_us(10.0); // A delay of 10 microseconds between each sensor output. Can be changed.
raizel_varun 0:ebdf4f859dca 123
raizel_varun 0:ebdf4f859dca 124 }
raizel_varun 0:ebdf4f859dca 125
raizel_varun 0:ebdf4f859dca 126
raizel_varun 0:ebdf4f859dca 127 //measurement of temperature
raizel_varun 0:ebdf4f859dca 128 //temperature measurement=> 4 thermistors, 1 temperature sensor
raizel_varun 0:ebdf4f859dca 129 //mux line 1=>temp sensor, mux lines 2 to 5 =>thermistors
raizel_varun 0:ebdf4f859dca 130
raizel_varun 0:ebdf4f859dca 131 for(LoopIterator=0; LoopIterator<5; LoopIterator++)
raizel_varun 0:ebdf4f859dca 132 {
greenroshks 2:edd107ea4740 133
raizel_varun 0:ebdf4f859dca 134 //following lines read the sensor values and stores them in 'SensorData' structure variable 'Sensor'
raizel_varun 0:ebdf4f859dca 135 Sensor.Temperature[LoopIterator]=(-90.7*3.3*TemperatureInput.read()+190.1543);
raizel_varun 0:ebdf4f859dca 136 voltage_thermistor=TemperatureInput.read()*3.3;//voltage across thermistor
raizel_varun 0:ebdf4f859dca 137 resistance_thermistor=24000*voltage_thermistor/(3.3-voltage_thermistor);//resistance of thermistor
greenroshks 2:edd107ea4740 138 if (LoopIterator==0)
greenroshks 2:edd107ea4740 139 {
sakthipriya 3:20647ff68b3c 140 printf(" \n\rTemp =%f",-90.7*3.3*TemperatureInput.read()+190.1543);
greenroshks 2:edd107ea4740 141 }
raizel_varun 0:ebdf4f859dca 142
raizel_varun 0:ebdf4f859dca 143 if(LoopIterator%2==0)
raizel_varun 0:ebdf4f859dca 144 {
raizel_varun 0:ebdf4f859dca 145 if(LoopIterator<1) //->corresponding to temperature sensor
raizel_varun 0:ebdf4f859dca 146 SensorQuantised.Temperature[(LoopIterator)/2]=quantiz(tstart,tstep,Sensor.Temperature[LoopIterator]);
raizel_varun 0:ebdf4f859dca 147
raizel_varun 0:ebdf4f859dca 148 else //->corresponding to thermistor
raizel_varun 0:ebdf4f859dca 149 {
raizel_varun 0:ebdf4f859dca 150 if(voltage_thermistor<1.378) //Temperature>12 degC
raizel_varun 0:ebdf4f859dca 151 Sensor.PanelTemperature[(LoopIterator-1)]=(3694/log(24.032242*resistance_thermistor));
raizel_varun 0:ebdf4f859dca 152
raizel_varun 0:ebdf4f859dca 153 else
raizel_varun 0:ebdf4f859dca 154 Sensor.PanelTemperature[(LoopIterator-1)]=(3365.4792/log(7.60404*resistance_thermistor));
raizel_varun 0:ebdf4f859dca 155
raizel_varun 0:ebdf4f859dca 156
raizel_varun 0:ebdf4f859dca 157 SensorQuantised.PanelTemperature[(LoopIterator-1)/2]=quantiz(tstart_thermistor,tstep_thermistor,Sensor.PanelTemperature[(LoopIterator-1)]);
raizel_varun 0:ebdf4f859dca 158
raizel_varun 0:ebdf4f859dca 159 }
raizel_varun 0:ebdf4f859dca 160
raizel_varun 0:ebdf4f859dca 161 }
raizel_varun 0:ebdf4f859dca 162
raizel_varun 0:ebdf4f859dca 163 else
raizel_varun 0:ebdf4f859dca 164 {
raizel_varun 0:ebdf4f859dca 165 if(LoopIterator<1)
raizel_varun 0:ebdf4f859dca 166 SensorQuantised.Temperature[(LoopIterator)/2]=SensorQuantised.Temperature[(LoopIterator)/2]<<4+quantiz(tstart,tstep,Sensor.Temperature[LoopIterator]);
raizel_varun 0:ebdf4f859dca 167
raizel_varun 0:ebdf4f859dca 168 else
raizel_varun 0:ebdf4f859dca 169 {
raizel_varun 0:ebdf4f859dca 170 if(voltage_thermistor<1.378) //Temperature>12 degC
raizel_varun 0:ebdf4f859dca 171 Sensor.PanelTemperature[LoopIterator-1]=(3694/log(24.032242*resistance_thermistor));
raizel_varun 0:ebdf4f859dca 172
raizel_varun 0:ebdf4f859dca 173
raizel_varun 0:ebdf4f859dca 174 else
raizel_varun 0:ebdf4f859dca 175 Sensor.PanelTemperature[LoopIterator-1]=(3365.4792/log(7.60404*resistance_thermistor));
raizel_varun 0:ebdf4f859dca 176
raizel_varun 0:ebdf4f859dca 177 SensorQuantised.PanelTemperature[(LoopIterator-1)/2]=SensorQuantised.PanelTemperature[(LoopIterator-1)/2]<<4+quantiz(tstart_thermistor,tstep_thermistor,Sensor.PanelTemperature[LoopIterator-1]);
raizel_varun 0:ebdf4f859dca 178
raizel_varun 0:ebdf4f859dca 179 }
raizel_varun 0:ebdf4f859dca 180
raizel_varun 0:ebdf4f859dca 181 }
raizel_varun 0:ebdf4f859dca 182
raizel_varun 0:ebdf4f859dca 183 strcpy(SensorQuantised.Voltage,"green");
raizel_varun 0:ebdf4f859dca 184
raizel_varun 0:ebdf4f859dca 185 // The following lines are used to iterate the select lines from 0 to 4
raizel_varun 0:ebdf4f859dca 186
raizel_varun 0:ebdf4f859dca 187 //following is an algorithm similar to counting binary numbers of 4 bit
raizel_varun 0:ebdf4f859dca 188 for(SelectLineIterator=3;SelectLineIterator>=0;SelectLineIterator--)
raizel_varun 0:ebdf4f859dca 189 {
raizel_varun 0:ebdf4f859dca 190 if(SelectLinesC[SelectLineIterator]==0)
raizel_varun 0:ebdf4f859dca 191 {
raizel_varun 0:ebdf4f859dca 192 SelectLinesC[SelectLineIterator]=1;
raizel_varun 0:ebdf4f859dca 193 break;
raizel_varun 0:ebdf4f859dca 194 }
raizel_varun 0:ebdf4f859dca 195 else SelectLinesC[SelectLineIterator]=0;
raizel_varun 0:ebdf4f859dca 196
raizel_varun 0:ebdf4f859dca 197 }
raizel_varun 0:ebdf4f859dca 198
raizel_varun 0:ebdf4f859dca 199
raizel_varun 0:ebdf4f859dca 200 wait_us(10.0); // A delay of 10 microseconds between each sensor output. Can be changed.
raizel_varun 0:ebdf4f859dca 201
raizel_varun 0:ebdf4f859dca 202 }
raizel_varun 0:ebdf4f859dca 203
raizel_varun 0:ebdf4f859dca 204
raizel_varun 0:ebdf4f859dca 205
raizel_varun 0:ebdf4f859dca 206
raizel_varun 0:ebdf4f859dca 207
raizel_varun 0:ebdf4f859dca 208 //update magnetometer data->
raizel_varun 0:ebdf4f859dca 209 //populate values in structure variable 'Sensor' from data to be given by Green
raizel_varun 0:ebdf4f859dca 210 SensorQuantised.AngularSpeed[0]=quantiz(AngularSpeed_start,AngularSpeed_step,Sensor.AngularSpeed[1]);
raizel_varun 0:ebdf4f859dca 211 SensorQuantised.AngularSpeed[0]=SensorQuantised.AngularSpeed[0]<<4+quantiz(AngularSpeed_start,AngularSpeed_step,Sensor.AngularSpeed[0]);
raizel_varun 0:ebdf4f859dca 212 SensorQuantised.AngularSpeed[1]=quantiz(AngularSpeed_start,AngularSpeed_step,Sensor.AngularSpeed[2]);
raizel_varun 0:ebdf4f859dca 213
raizel_varun 0:ebdf4f859dca 214 //update gyro data->
raizel_varun 0:ebdf4f859dca 215 //populate values in structure variable 'Sensor' from data to be given by Green
raizel_varun 0:ebdf4f859dca 216 SensorQuantised.Bnewvalue[0]=quantiz(Bnewvalue_start,Bnewvalue_step,Sensor.Bnewvalue[1]);
raizel_varun 0:ebdf4f859dca 217 SensorQuantised.Bnewvalue[0]=SensorQuantised.Bnewvalue[0]<<4+quantiz(Bnewvalue_start,Bnewvalue_step,Sensor.Bnewvalue[0]);
raizel_varun 0:ebdf4f859dca 218 SensorQuantised.Bnewvalue[1]=quantiz(Bnewvalue_start,Bnewvalue_step,Sensor.Bnewvalue[2]);
raizel_varun 0:ebdf4f859dca 219
raizel_varun 0:ebdf4f859dca 220
raizel_varun 0:ebdf4f859dca 221
raizel_varun 0:ebdf4f859dca 222
raizel_varun 0:ebdf4f859dca 223 //update beacon structure
raizel_varun 0:ebdf4f859dca 224 init_beacon(&Shortbeacon,SensorQuantised);//Shortbeacon is passed
raizel_varun 0:ebdf4f859dca 225
raizel_varun 0:ebdf4f859dca 226
raizel_varun 0:ebdf4f859dca 227 }
raizel_varun 0:ebdf4f859dca 228
raizel_varun 0:ebdf4f859dca 229
raizel_varun 0:ebdf4f859dca 230