main.cpp

00001 // J. Bradshaw 20141119
00002 // Program for testing I/O on mbed ES200 breakout board
00003 #include "mbed.h"
00004 #include "SERVOGEN.h"   //uses SERVOGEN.h library for generating servo pulses
00005 #include "rtos.h"
00006 
00007 #define PI 3.1415926
00008 
00009 DigitalOut led1(LED1);
00010 DigitalOut led3(LED3);
00011 
00012 //PC serial connection
00013 Serial pc(USBTX, USBRX);    //tx, rx via USB connection
00014 Serial DB9_female(p9, p10); //DB9 serial female
00015 Serial DB9_male(p13, p14); //DB9 serial male
00016 
00017 //Port J2
00018 BusOut bus_J2(p5, p6, p7, p8, p11);
00019 
00020 //header for J2
00021 DigitalOut J2_p5(p5);
00022 DigitalOut J2_p6(p6);
00023 DigitalOut J2_p7(p7);
00024 DigitalOut J2_p8(p8);
00025 DigitalOut J2_p11(p11);
00026 
00027 //Analog port in J3
00028 AnalogIn    J3_P16(p16);
00029 AnalogIn    J3_P17(p17);
00030 AnalogIn    J3_P18(p18);
00031 AnalogIn    J3_P19(p19);
00032 AnalogIn    J3_P20(p20);
00033 
00034 // servo ports p21 - p24
00035 SERVOGEN sv1(p21);
00036 SERVOGEN sv2(p22);
00037 SERVOGEN sv3(p23);
00038 SERVOGEN sv4(p24);
00039 
00040 DigitalOut mot1_ph1(p28);
00041 DigitalOut mot1_ph2(p27);
00042 PwmOut mot_en1(p25);
00043 
00044 DigitalOut mot2_ph1(p29);
00045 DigitalOut mot2_ph2(p30);
00046 PwmOut mot_en2(p26);
00047 //Motor control routine for PWM on 5 pin motor driver header
00048 // drv_num is 1 or 2 (defaults to 1, anything but 2)
00049 // dc is signed duty cycle (+/-1.0)
00050 
00051 void mot_control(int drv_num, float dc){        
00052     if(dc>1.0)
00053         dc=1.0;
00054     if(dc<-1.0)
00055         dc=-1.0;
00056     
00057     if(drv_num != 2){           
00058         if(dc > 0.0){
00059             mot1_ph2 = 0;
00060             mot1_ph1 = 1;
00061             mot_en1 = dc;
00062         }
00063         else if(dc < -0.0){
00064             mot1_ph1 = 0;
00065             mot1_ph2 = 1;
00066             mot_en1 = abs(dc);
00067         }
00068         else{
00069             mot1_ph1 = 0;
00070             mot1_ph2 = 0;
00071             mot_en1 = 0.0;
00072         }
00073     }                
00074     else{
00075         if(dc > 0.0){
00076             mot2_ph2 = 0;
00077             mot2_ph1 = 1;
00078             mot_en2 = dc;
00079         }
00080         else if(dc < -0.0){
00081             mot2_ph1 = 0;
00082             mot2_ph2 = 1;
00083             mot_en2 = abs(dc);
00084         }
00085         else{
00086             mot2_ph1 = 0;
00087             mot2_ph2 = 0;
00088             mot_en2 = 0.0;
00089         }
00090     }                   
00091 }
00092 
00093 void mot1_thread(void const *args){
00094         while(1){
00095         // Run PWM and servo command signals
00096         for(float cycle=0;cycle<2.0*PI;cycle+=.078){
00097             sv1 = 1500 + (int)(sin(cycle)*500);
00098             sv2 = 1500 + (int)(cos(cycle)*500);
00099             sv3 = 1500 + (int)(sin(cycle)*500);
00100             sv4 = 1500 + (int)(sin(cycle)*500);
00101         
00102             mot_control(1, .85*sin(cycle));
00103             mot_control(2, .85*cos(cycle));
00104                         
00105             pc.printf("cycle = %.2f  \r", cycle);
00106             Thread::wait(10);
00107         } 
00108     }//while(1)
00109 }
00110 
00111 void digOut_thread(void const *args){
00112     //toggle bus I/O for testing
00113     while(1){
00114         bus_J2 = 0x1f;
00115         Thread::wait(500);
00116         bus_J2 = 0xAA;
00117         Thread::wait(100);
00118         bus_J2 = 0x55;
00119         Thread::wait(100);
00120         bus_J2 = 0xAA;
00121         Thread::wait(100);
00122         bus_J2 = 0x55;
00123         Thread::wait(100);        
00124         bus_J2 = 0; //all off
00125         Thread::wait(500);    
00126     }
00127 }
00128 
00129 void led1_flash(void const *args){
00130     while(1){
00131         led1 = !led1;
00132         Thread::wait(500);
00133     }
00134 }
00135 //------------ Main ------------------------------
00136 int main() {
00137     float analogVal[6];
00138     
00139     Thread t1(mot1_thread);
00140     Thread t2(digOut_thread);
00141     Thread t3(led1_flash);
00142     
00143     pc.baud(9600);
00144     DB9_female.baud(9600);
00145     DB9_male.baud(9600);
00146     mot_en1.period_us(20);
00147     mot_en2.period_us(20);
00148     
00149     while(1) {        
00150         analogVal[0] = J3_P16;
00151         analogVal[1] = J3_P17;
00152         analogVal[2] = J3_P18;
00153         analogVal[3] = J3_P19;
00154         analogVal[4] = J3_P20;
00155         pc.printf("%.2f %.2f %.2f %.2f %.2f \r\n", analogVal[0],analogVal[1],analogVal[2],analogVal[3],analogVal[4]);
00156         DB9_female.printf("Female %.2f %.2f %.2f %.2f %.2f \r\n", analogVal[0],analogVal[1],analogVal[2],analogVal[3],analogVal[4]);        DB9_male.printf("Male %.2f %.2f %.2f %.2f %.2f \r\n", analogVal[0],analogVal[1],analogVal[2],analogVal[3],analogVal[4]);                
00157             
00158         pc.printf("\r\n");
00159     }
00160 }