USNA ES305
/
TwoTank
...
Diff: main.cpp
- Revision:
- 0:722b391fe228
- Child:
- 1:daaa91186ace
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Aug 05 19:22:19 2014 +0000 @@ -0,0 +1,204 @@ +#include "mbed.h" +// APIs +Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc +SPI spi_max1270(p5, p6, p7); +SPI spi(p5, p6, p7); +DigitalOut max1270_cs(p8); //MAX1270 ADC CS +//DigitalOut max522_cs(p11); //MAX522 DAC CS +DigitalOut mot1_ph1(p21); +DigitalOut mot1_ph2(p22); +PwmOut mot_en1(p23); + +/*DigitalOut mot2_ph1(p24); +DigitalOut mot2_ph2(p25); +PwmOut mot_en2(p26);*/ + +int read_max1270(int chan, int range, int bipol){ + int cword=0x80; //set the start bit + + spi_max1270.frequency(10000000); + spi_max1270.format(8, 0); // 8 data bits, CPOL0, and CPHA0 (datasheet Digital Interface) + + cword |= (chan << 4); //shift channel + cword |= (range << 3); + cword |= (bipol << 2); + + max1270_cs = 0; + + spi_max1270.write(cword); + wait_us(15); //15us + spi_max1270.format(12, 3); + + int result = spi_max1270.write(0); + + max1270_cs = 1; + spi_max1270.format(8, 0); + return result; +} + +float read_max1270_volts(int chan, int range, int bipol){ + float rangevolts=0.0; + float volts=0.0; + int adc_res; + + //read the ADC converter + adc_res = read_max1270(chan, range, bipol) & 0xFFF; + + //Determine the voltage range + if(range) //RNG bit + rangevolts=10.0; + else + rangevolts=5.0; + + //bi-polar input range + if(bipol){ //BIP is set, input is +/- + if(adc_res < 0x800){ //if result was positive + volts = ((float)adc_res/0x7FF) * rangevolts; + } + else{ //result was negative + volts = -(-((float)adc_res/0x7FF) * rangevolts) - (rangevolts * 2.0); + } + } + else{ //input is positive polarity only + volts = ((float)adc_res/0xFFF) * rangevolts; + } + + return volts; +} + +//Motor control routine for PWM on 5 pin motor driver header +// drv_num is 1 or 2 (defaults to 1, anything but 2) +// dc is signed duty cycle (+/-1.0) + +/*void mot_control(int drv_num, float dc){ + if(dc>1.0) + dc=1.0; + if(dc<-1.0) + dc=-1.0; + + if(drv_num != 2){ + if(dc > 0.0){ + mot1_ph2 = 0; + mot1_ph1 = 1; + mot_en1 = dc; + } + else if(dc < -0.0){ + mot1_ph1 = 0; + mot1_ph2 = 1; + mot_en1 = abs(dc); + } + else{ + mot1_ph1 = 0; + mot1_ph2 = 0; + mot_en1 = 0.0; + } + } + else{ + if(dc > 0.0){ + mot2_ph2 = 0; + mot2_ph1 = 1; + mot_en2 = dc; + } + else if(dc < -0.0){ + mot2_ph1 = 0; + mot2_ph2 = 1; + mot_en2 = abs(dc); + } + else{ + mot2_ph1 = 0; + mot2_ph2 = 0; + mot_en2 = 0.0; + } + } +}*/ + +//------- MAX522 routines --------------------------------- +/*void write_max522(int chan, float volts){ + int cmd=0x20; //set UB3 + int data_word = (int)((volts/5.0) * 256.0); + if(chan != 2) + cmd |= 0x01; //set DAC A out + else + cmd |= 0x02; //set DACB out + + // pc.printf("cmd=0x%4X data_word=0x%4X \r\n", cmd, data_word); + + spi.format(8, 0); + spi.frequency(2000000); + max522_cs = 0; + spi.write(cmd & 0xFF); + spi.write(data_word & 0xFF); + max522_cs = 1; + spi.frequency(10000000); +}*/ + +float Tank1,Tank2,dt,h1,h2; +float Ts = 0.5; // 1/Ts Hz +float c1 = 15.0/(2.77+1.24); // cm/V +float c2 = 11.5/(3.58-0.79); // cm/V +float v10 = -0.52; // V +float v20 = -3.35; // V +Timer t; + +float cntr; +float dc; +int main () +{ + pc.baud(921600); + mot_en1.period_us(50); + max1270_cs = 1; + //Ts = 0.1; + //dc = 0.2; + cntr = 0.0; + while(cntr*Ts <= 120) { + t.start(); // start measuring comp time + // Read pressure sensors + + Tank1 = read_max1270_volts(0, 1, 1); + h1 = c1*(Tank1 - v10); + Tank2 = read_max1270_volts(1, 1, 1); + h2 = c2*(Tank2 - v20); + // drive motor + //if (cntr*Ts <= 0.05) { + // dc = 0.0; + //} else if (cntr*Ts <= 0.2) { + // dc = 0.3; + //} else { + // dc = 0.7; + //} + + if (cntr*Ts <= 5) { + dc = 0.0; + } else if (cntr*Ts <= 110) { + dc = -0.75; + } else { + dc = 0.0; + } + + //dc = 0.75; + //mot_control(1,-dc); + + if(dc > 0.0){ + mot1_ph2 = 0; + mot1_ph1 = 1; + mot_en1 = dc; + } + else if(dc < -0.0){ + mot1_ph1 = 0; + mot1_ph2 = 1; + mot_en1 = abs(dc);} + + + t.stop(); // end measuring comp time + //printf("%f\n\r",t.read()); + dt = Ts-t.read(); + //printf("%f\n\r",dt); + pc.printf("%5.2f %5.2f %5.2f %5.2f %5.2f %5.2f \n\r",cntr*Ts,Tank1,Tank2,h1,h2,dc); + //printf("%5.2f \n\r",angleNew); + t.reset(); + cntr=cntr+1; + wait(dt); // wait to ensure sampling period set by Ts + }//while + mot1_ph2 = 0; + mot_en1 = 0.0; +}//main \ No newline at end of file