USNA ES305 / Mbed 2 deprecated TwoTank

...

Dependencies:   mbed Matrix

main.cpp@0:722b391fe228, 2014-08-05 (annotated)

Committer:
riobrien
Date:
Tue Aug 05 19:22:19 2014 +0000
Revision:
0:722b391fe228
Child:
1:daaa91186ace
...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
riobrien 0:722b391fe228 1 #include "mbed.h"
riobrien 0:722b391fe228 2 // APIs
riobrien 0:722b391fe228 3 Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
riobrien 0:722b391fe228 4 SPI spi_max1270(p5, p6, p7);
riobrien 0:722b391fe228 5 SPI spi(p5, p6, p7);
riobrien 0:722b391fe228 6 DigitalOut max1270_cs(p8); //MAX1270 ADC CS
riobrien 0:722b391fe228 7 //DigitalOut max522_cs(p11); //MAX522 DAC CS
riobrien 0:722b391fe228 8 DigitalOut mot1_ph1(p21);
riobrien 0:722b391fe228 9 DigitalOut mot1_ph2(p22);
riobrien 0:722b391fe228 10 PwmOut mot_en1(p23);
riobrien 0:722b391fe228 11
riobrien 0:722b391fe228 12 /*DigitalOut mot2_ph1(p24);
riobrien 0:722b391fe228 13 DigitalOut mot2_ph2(p25);
riobrien 0:722b391fe228 14 PwmOut mot_en2(p26);*/
riobrien 0:722b391fe228 15
riobrien 0:722b391fe228 16 int read_max1270(int chan, int range, int bipol){
riobrien 0:722b391fe228 17 int cword=0x80; //set the start bit
riobrien 0:722b391fe228 18
riobrien 0:722b391fe228 19 spi_max1270.frequency(10000000);
riobrien 0:722b391fe228 20 spi_max1270.format(8, 0); // 8 data bits, CPOL0, and CPHA0 (datasheet Digital Interface)
riobrien 0:722b391fe228 21
riobrien 0:722b391fe228 22 cword |= (chan << 4); //shift channel
riobrien 0:722b391fe228 23 cword |= (range << 3);
riobrien 0:722b391fe228 24 cword |= (bipol << 2);
riobrien 0:722b391fe228 25
riobrien 0:722b391fe228 26 max1270_cs = 0;
riobrien 0:722b391fe228 27
riobrien 0:722b391fe228 28 spi_max1270.write(cword);
riobrien 0:722b391fe228 29 wait_us(15); //15us
riobrien 0:722b391fe228 30 spi_max1270.format(12, 3);
riobrien 0:722b391fe228 31
riobrien 0:722b391fe228 32 int result = spi_max1270.write(0);
riobrien 0:722b391fe228 33
riobrien 0:722b391fe228 34 max1270_cs = 1;
riobrien 0:722b391fe228 35 spi_max1270.format(8, 0);
riobrien 0:722b391fe228 36 return result;
riobrien 0:722b391fe228 37 }
riobrien 0:722b391fe228 38
riobrien 0:722b391fe228 39 float read_max1270_volts(int chan, int range, int bipol){
riobrien 0:722b391fe228 40 float rangevolts=0.0;
riobrien 0:722b391fe228 41 float volts=0.0;
riobrien 0:722b391fe228 42 int adc_res;
riobrien 0:722b391fe228 43
riobrien 0:722b391fe228 44 //read the ADC converter
riobrien 0:722b391fe228 45 adc_res = read_max1270(chan, range, bipol) & 0xFFF;
riobrien 0:722b391fe228 46
riobrien 0:722b391fe228 47 //Determine the voltage range
riobrien 0:722b391fe228 48 if(range) //RNG bit
riobrien 0:722b391fe228 49 rangevolts=10.0;
riobrien 0:722b391fe228 50 else
riobrien 0:722b391fe228 51 rangevolts=5.0;
riobrien 0:722b391fe228 52
riobrien 0:722b391fe228 53 //bi-polar input range
riobrien 0:722b391fe228 54 if(bipol){ //BIP is set, input is +/-
riobrien 0:722b391fe228 55 if(adc_res < 0x800){ //if result was positive
riobrien 0:722b391fe228 56 volts = ((float)adc_res/0x7FF) * rangevolts;
riobrien 0:722b391fe228 57 }
riobrien 0:722b391fe228 58 else{ //result was negative
riobrien 0:722b391fe228 59 volts = -(-((float)adc_res/0x7FF) * rangevolts) - (rangevolts * 2.0);
riobrien 0:722b391fe228 60 }
riobrien 0:722b391fe228 61 }
riobrien 0:722b391fe228 62 else{ //input is positive polarity only
riobrien 0:722b391fe228 63 volts = ((float)adc_res/0xFFF) * rangevolts;
riobrien 0:722b391fe228 64 }
riobrien 0:722b391fe228 65
riobrien 0:722b391fe228 66 return volts;
riobrien 0:722b391fe228 67 }
riobrien 0:722b391fe228 68
riobrien 0:722b391fe228 69 //Motor control routine for PWM on 5 pin motor driver header
riobrien 0:722b391fe228 70 // drv_num is 1 or 2 (defaults to 1, anything but 2)
riobrien 0:722b391fe228 71 // dc is signed duty cycle (+/-1.0)
riobrien 0:722b391fe228 72
riobrien 0:722b391fe228 73 /*void mot_control(int drv_num, float dc){
riobrien 0:722b391fe228 74 if(dc>1.0)
riobrien 0:722b391fe228 75 dc=1.0;
riobrien 0:722b391fe228 76 if(dc<-1.0)
riobrien 0:722b391fe228 77 dc=-1.0;
riobrien 0:722b391fe228 78
riobrien 0:722b391fe228 79 if(drv_num != 2){
riobrien 0:722b391fe228 80 if(dc > 0.0){
riobrien 0:722b391fe228 81 mot1_ph2 = 0;
riobrien 0:722b391fe228 82 mot1_ph1 = 1;
riobrien 0:722b391fe228 83 mot_en1 = dc;
riobrien 0:722b391fe228 84 }
riobrien 0:722b391fe228 85 else if(dc < -0.0){
riobrien 0:722b391fe228 86 mot1_ph1 = 0;
riobrien 0:722b391fe228 87 mot1_ph2 = 1;
riobrien 0:722b391fe228 88 mot_en1 = abs(dc);
riobrien 0:722b391fe228 89 }
riobrien 0:722b391fe228 90 else{
riobrien 0:722b391fe228 91 mot1_ph1 = 0;
riobrien 0:722b391fe228 92 mot1_ph2 = 0;
riobrien 0:722b391fe228 93 mot_en1 = 0.0;
riobrien 0:722b391fe228 94 }
riobrien 0:722b391fe228 95 }
riobrien 0:722b391fe228 96 else{
riobrien 0:722b391fe228 97 if(dc > 0.0){
riobrien 0:722b391fe228 98 mot2_ph2 = 0;
riobrien 0:722b391fe228 99 mot2_ph1 = 1;
riobrien 0:722b391fe228 100 mot_en2 = dc;
riobrien 0:722b391fe228 101 }
riobrien 0:722b391fe228 102 else if(dc < -0.0){
riobrien 0:722b391fe228 103 mot2_ph1 = 0;
riobrien 0:722b391fe228 104 mot2_ph2 = 1;
riobrien 0:722b391fe228 105 mot_en2 = abs(dc);
riobrien 0:722b391fe228 106 }
riobrien 0:722b391fe228 107 else{
riobrien 0:722b391fe228 108 mot2_ph1 = 0;
riobrien 0:722b391fe228 109 mot2_ph2 = 0;
riobrien 0:722b391fe228 110 mot_en2 = 0.0;
riobrien 0:722b391fe228 111 }
riobrien 0:722b391fe228 112 }
riobrien 0:722b391fe228 113 }*/
riobrien 0:722b391fe228 114
riobrien 0:722b391fe228 115 //------- MAX522 routines ---------------------------------
riobrien 0:722b391fe228 116 /*void write_max522(int chan, float volts){
riobrien 0:722b391fe228 117 int cmd=0x20; //set UB3
riobrien 0:722b391fe228 118 int data_word = (int)((volts/5.0) * 256.0);
riobrien 0:722b391fe228 119 if(chan != 2)
riobrien 0:722b391fe228 120 cmd |= 0x01; //set DAC A out
riobrien 0:722b391fe228 121 else
riobrien 0:722b391fe228 122 cmd |= 0x02; //set DACB out
riobrien 0:722b391fe228 123
riobrien 0:722b391fe228 124 // pc.printf("cmd=0x%4X data_word=0x%4X \r\n", cmd, data_word);
riobrien 0:722b391fe228 125
riobrien 0:722b391fe228 126 spi.format(8, 0);
riobrien 0:722b391fe228 127 spi.frequency(2000000);
riobrien 0:722b391fe228 128 max522_cs = 0;
riobrien 0:722b391fe228 129 spi.write(cmd & 0xFF);
riobrien 0:722b391fe228 130 spi.write(data_word & 0xFF);
riobrien 0:722b391fe228 131 max522_cs = 1;
riobrien 0:722b391fe228 132 spi.frequency(10000000);
riobrien 0:722b391fe228 133 }*/
riobrien 0:722b391fe228 134
riobrien 0:722b391fe228 135 float Tank1,Tank2,dt,h1,h2;
riobrien 0:722b391fe228 136 float Ts = 0.5; // 1/Ts Hz
riobrien 0:722b391fe228 137 float c1 = 15.0/(2.77+1.24); // cm/V
riobrien 0:722b391fe228 138 float c2 = 11.5/(3.58-0.79); // cm/V
riobrien 0:722b391fe228 139 float v10 = -0.52; // V
riobrien 0:722b391fe228 140 float v20 = -3.35; // V
riobrien 0:722b391fe228 141 Timer t;
riobrien 0:722b391fe228 142
riobrien 0:722b391fe228 143 float cntr;
riobrien 0:722b391fe228 144 float dc;
riobrien 0:722b391fe228 145 int main ()
riobrien 0:722b391fe228 146 {
riobrien 0:722b391fe228 147 pc.baud(921600);
riobrien 0:722b391fe228 148 mot_en1.period_us(50);
riobrien 0:722b391fe228 149 max1270_cs = 1;
riobrien 0:722b391fe228 150 //Ts = 0.1;
riobrien 0:722b391fe228 151 //dc = 0.2;
riobrien 0:722b391fe228 152 cntr = 0.0;
riobrien 0:722b391fe228 153 while(cntr*Ts <= 120) {
riobrien 0:722b391fe228 154 t.start(); // start measuring comp time
riobrien 0:722b391fe228 155 // Read pressure sensors
riobrien 0:722b391fe228 156
riobrien 0:722b391fe228 157 Tank1 = read_max1270_volts(0, 1, 1);
riobrien 0:722b391fe228 158 h1 = c1*(Tank1 - v10);
riobrien 0:722b391fe228 159 Tank2 = read_max1270_volts(1, 1, 1);
riobrien 0:722b391fe228 160 h2 = c2*(Tank2 - v20);
riobrien 0:722b391fe228 161 // drive motor
riobrien 0:722b391fe228 162 //if (cntr*Ts <= 0.05) {
riobrien 0:722b391fe228 163 // dc = 0.0;
riobrien 0:722b391fe228 164 //} else if (cntr*Ts <= 0.2) {
riobrien 0:722b391fe228 165 // dc = 0.3;
riobrien 0:722b391fe228 166 //} else {
riobrien 0:722b391fe228 167 // dc = 0.7;
riobrien 0:722b391fe228 168 //}
riobrien 0:722b391fe228 169
riobrien 0:722b391fe228 170 if (cntr*Ts <= 5) {
riobrien 0:722b391fe228 171 dc = 0.0;
riobrien 0:722b391fe228 172 } else if (cntr*Ts <= 110) {
riobrien 0:722b391fe228 173 dc = -0.75;
riobrien 0:722b391fe228 174 } else {
riobrien 0:722b391fe228 175 dc = 0.0;
riobrien 0:722b391fe228 176 }
riobrien 0:722b391fe228 177
riobrien 0:722b391fe228 178 //dc = 0.75;
riobrien 0:722b391fe228 179 //mot_control(1,-dc);
riobrien 0:722b391fe228 180
riobrien 0:722b391fe228 181 if(dc > 0.0){
riobrien 0:722b391fe228 182 mot1_ph2 = 0;
riobrien 0:722b391fe228 183 mot1_ph1 = 1;
riobrien 0:722b391fe228 184 mot_en1 = dc;
riobrien 0:722b391fe228 185 }
riobrien 0:722b391fe228 186 else if(dc < -0.0){
riobrien 0:722b391fe228 187 mot1_ph1 = 0;
riobrien 0:722b391fe228 188 mot1_ph2 = 1;
riobrien 0:722b391fe228 189 mot_en1 = abs(dc);}
riobrien 0:722b391fe228 190
riobrien 0:722b391fe228 191
riobrien 0:722b391fe228 192 t.stop(); // end measuring comp time
riobrien 0:722b391fe228 193 //printf("%f\n\r",t.read());
riobrien 0:722b391fe228 194 dt = Ts-t.read();
riobrien 0:722b391fe228 195 //printf("%f\n\r",dt);
riobrien 0:722b391fe228 196 pc.printf("%5.2f %5.2f %5.2f %5.2f %5.2f %5.2f \n\r",cntr*Ts,Tank1,Tank2,h1,h2,dc);
riobrien 0:722b391fe228 197 //printf("%5.2f \n\r",angleNew);
riobrien 0:722b391fe228 198 t.reset();
riobrien 0:722b391fe228 199 cntr=cntr+1;
riobrien 0:722b391fe228 200 wait(dt); // wait to ensure sampling period set by Ts
riobrien 0:722b391fe228 201 }//while
riobrien 0:722b391fe228 202 mot1_ph2 = 0;
riobrien 0:722b391fe228 203 mot_en1 = 0.0;
riobrien 0:722b391fe228 204 }//main