Test Program for the ES20X mbed board
Dependencies: ServoOut mbed-rtos mbed
Fork of mbed_ES20X_Thread_Test by
main.cpp
- Committer:
- jebradshaw
- Date:
- 2016-01-08
- Revision:
- 0:ff2198a98673
- Child:
- 1:31677f9c113f
File content as of revision 0:ff2198a98673:
// J. Bradshaw 20141119 // Program for testing I/O on mbed ES200 breakout board #include "mbed.h" #include "SERVOGEN.h" //uses SERVOGEN.h library for generating servo pulses #include "rtos.h" #define PI 3.1415926 DigitalOut led1(LED1); DigitalOut led3(LED3); //PC serial connection Serial pc(USBTX, USBRX); //tx, rx via USB connection Serial DB9_female(p9, p10); //DB9 serial female Serial DB9_male(p13, p14); //DB9 serial male //Port J2 BusOut bus_J2(p5, p6, p7, p8, p11); //header for J2 DigitalOut J2_p5(p5); DigitalOut J2_p6(p6); DigitalOut J2_p7(p7); DigitalOut J2_p8(p8); DigitalOut J2_p11(p11); //Analog port in J3 AnalogIn J3_P16(p16); AnalogIn J3_P17(p17); AnalogIn J3_P18(p18); AnalogIn J3_P19(p19); AnalogIn J3_P20(p20); // servo ports p21 - p24 SERVOGEN sv1(p21); SERVOGEN sv2(p22); SERVOGEN sv3(p23); SERVOGEN sv4(p24); DigitalOut mot1_ph1(p28); DigitalOut mot1_ph2(p27); PwmOut mot_en1(p25); DigitalOut mot2_ph1(p29); DigitalOut mot2_ph2(p30); PwmOut mot_en2(p26); //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; } } } void mot1_thread(void const *args){ while(1){ // Run PWM and servo command signals for(float cycle=0;cycle<2.0*PI;cycle+=.078){ sv1 = 1500 + (int)(sin(cycle)*500); sv2 = 1500 + (int)(cos(cycle)*500); sv3 = 1500 + (int)(sin(cycle)*500); sv4 = 1500 + (int)(sin(cycle)*500); mot_control(1, .85*sin(cycle)); mot_control(2, .85*cos(cycle)); pc.printf("cycle = %.2f \r", cycle); Thread::wait(10); } }//while(1) } void digOut_thread(void const *args){ //toggle bus I/O for testing while(1){ bus_J2 = 0x1f; Thread::wait(500); bus_J2 = 0xAA; Thread::wait(100); bus_J2 = 0x55; Thread::wait(100); bus_J2 = 0xAA; Thread::wait(100); bus_J2 = 0x55; Thread::wait(100); bus_J2 = 0; //all off Thread::wait(500); } } void led1_flash(void const *args){ while(1){ led1 = !led1; Thread::wait(500); } } //------------ Main ------------------------------ int main() { float analogVal[6]; Thread t1(mot1_thread); Thread t2(digOut_thread); Thread t3(led1_flash); pc.baud(9600); DB9_female.baud(9600); DB9_male.baud(9600); mot_en1.period_us(20); mot_en2.period_us(20); while(1) { analogVal[0] = J3_P16; analogVal[1] = J3_P17; analogVal[2] = J3_P18; analogVal[3] = J3_P19; analogVal[4] = J3_P20; pc.printf("%.2f %.2f %.2f %.2f %.2f \r\n", analogVal[0],analogVal[1],analogVal[2],analogVal[3],analogVal[4]); 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]); pc.printf("\r\n"); } }