main.cpp

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