Test Program for the ES20X mbed board
Dependencies: ServoOut mbed-rtos mbed
Fork of mbed_ES20X_Thread_Test by
main.cpp@0:ff2198a98673, 2016-01-08 (annotated)
- Committer:
- jebradshaw
- Date:
- Fri Jan 08 16:53:32 2016 +0000
- Revision:
- 0:ff2198a98673
- Child:
- 1:31677f9c113f
Test Program for the ES20X mbed board (servos, DC motor control, analog inputs, etc)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jebradshaw | 0:ff2198a98673 | 1 | // J. Bradshaw 20141119 |
jebradshaw | 0:ff2198a98673 | 2 | // Program for testing I/O on mbed ES200 breakout board |
jebradshaw | 0:ff2198a98673 | 3 | #include "mbed.h" |
jebradshaw | 0:ff2198a98673 | 4 | #include "SERVOGEN.h" //uses SERVOGEN.h library for generating servo pulses |
jebradshaw | 0:ff2198a98673 | 5 | #include "rtos.h" |
jebradshaw | 0:ff2198a98673 | 6 | |
jebradshaw | 0:ff2198a98673 | 7 | #define PI 3.1415926 |
jebradshaw | 0:ff2198a98673 | 8 | |
jebradshaw | 0:ff2198a98673 | 9 | DigitalOut led1(LED1); |
jebradshaw | 0:ff2198a98673 | 10 | DigitalOut led3(LED3); |
jebradshaw | 0:ff2198a98673 | 11 | |
jebradshaw | 0:ff2198a98673 | 12 | //PC serial connection |
jebradshaw | 0:ff2198a98673 | 13 | Serial pc(USBTX, USBRX); //tx, rx via USB connection |
jebradshaw | 0:ff2198a98673 | 14 | Serial DB9_female(p9, p10); //DB9 serial female |
jebradshaw | 0:ff2198a98673 | 15 | Serial DB9_male(p13, p14); //DB9 serial male |
jebradshaw | 0:ff2198a98673 | 16 | |
jebradshaw | 0:ff2198a98673 | 17 | //Port J2 |
jebradshaw | 0:ff2198a98673 | 18 | BusOut bus_J2(p5, p6, p7, p8, p11); |
jebradshaw | 0:ff2198a98673 | 19 | |
jebradshaw | 0:ff2198a98673 | 20 | //header for J2 |
jebradshaw | 0:ff2198a98673 | 21 | DigitalOut J2_p5(p5); |
jebradshaw | 0:ff2198a98673 | 22 | DigitalOut J2_p6(p6); |
jebradshaw | 0:ff2198a98673 | 23 | DigitalOut J2_p7(p7); |
jebradshaw | 0:ff2198a98673 | 24 | DigitalOut J2_p8(p8); |
jebradshaw | 0:ff2198a98673 | 25 | DigitalOut J2_p11(p11); |
jebradshaw | 0:ff2198a98673 | 26 | |
jebradshaw | 0:ff2198a98673 | 27 | //Analog port in J3 |
jebradshaw | 0:ff2198a98673 | 28 | AnalogIn J3_P16(p16); |
jebradshaw | 0:ff2198a98673 | 29 | AnalogIn J3_P17(p17); |
jebradshaw | 0:ff2198a98673 | 30 | AnalogIn J3_P18(p18); |
jebradshaw | 0:ff2198a98673 | 31 | AnalogIn J3_P19(p19); |
jebradshaw | 0:ff2198a98673 | 32 | AnalogIn J3_P20(p20); |
jebradshaw | 0:ff2198a98673 | 33 | |
jebradshaw | 0:ff2198a98673 | 34 | // servo ports p21 - p24 |
jebradshaw | 0:ff2198a98673 | 35 | SERVOGEN sv1(p21); |
jebradshaw | 0:ff2198a98673 | 36 | SERVOGEN sv2(p22); |
jebradshaw | 0:ff2198a98673 | 37 | SERVOGEN sv3(p23); |
jebradshaw | 0:ff2198a98673 | 38 | SERVOGEN sv4(p24); |
jebradshaw | 0:ff2198a98673 | 39 | |
jebradshaw | 0:ff2198a98673 | 40 | DigitalOut mot1_ph1(p28); |
jebradshaw | 0:ff2198a98673 | 41 | DigitalOut mot1_ph2(p27); |
jebradshaw | 0:ff2198a98673 | 42 | PwmOut mot_en1(p25); |
jebradshaw | 0:ff2198a98673 | 43 | |
jebradshaw | 0:ff2198a98673 | 44 | DigitalOut mot2_ph1(p29); |
jebradshaw | 0:ff2198a98673 | 45 | DigitalOut mot2_ph2(p30); |
jebradshaw | 0:ff2198a98673 | 46 | PwmOut mot_en2(p26); |
jebradshaw | 0:ff2198a98673 | 47 | //Motor control routine for PWM on 5 pin motor driver header |
jebradshaw | 0:ff2198a98673 | 48 | // drv_num is 1 or 2 (defaults to 1, anything but 2) |
jebradshaw | 0:ff2198a98673 | 49 | // dc is signed duty cycle (+/-1.0) |
jebradshaw | 0:ff2198a98673 | 50 | |
jebradshaw | 0:ff2198a98673 | 51 | void mot_control(int drv_num, float dc){ |
jebradshaw | 0:ff2198a98673 | 52 | if(dc>1.0) |
jebradshaw | 0:ff2198a98673 | 53 | dc=1.0; |
jebradshaw | 0:ff2198a98673 | 54 | if(dc<-1.0) |
jebradshaw | 0:ff2198a98673 | 55 | dc=-1.0; |
jebradshaw | 0:ff2198a98673 | 56 | |
jebradshaw | 0:ff2198a98673 | 57 | if(drv_num != 2){ |
jebradshaw | 0:ff2198a98673 | 58 | if(dc > 0.0){ |
jebradshaw | 0:ff2198a98673 | 59 | mot1_ph2 = 0; |
jebradshaw | 0:ff2198a98673 | 60 | mot1_ph1 = 1; |
jebradshaw | 0:ff2198a98673 | 61 | mot_en1 = dc; |
jebradshaw | 0:ff2198a98673 | 62 | } |
jebradshaw | 0:ff2198a98673 | 63 | else if(dc < -0.0){ |
jebradshaw | 0:ff2198a98673 | 64 | mot1_ph1 = 0; |
jebradshaw | 0:ff2198a98673 | 65 | mot1_ph2 = 1; |
jebradshaw | 0:ff2198a98673 | 66 | mot_en1 = abs(dc); |
jebradshaw | 0:ff2198a98673 | 67 | } |
jebradshaw | 0:ff2198a98673 | 68 | else{ |
jebradshaw | 0:ff2198a98673 | 69 | mot1_ph1 = 0; |
jebradshaw | 0:ff2198a98673 | 70 | mot1_ph2 = 0; |
jebradshaw | 0:ff2198a98673 | 71 | mot_en1 = 0.0; |
jebradshaw | 0:ff2198a98673 | 72 | } |
jebradshaw | 0:ff2198a98673 | 73 | } |
jebradshaw | 0:ff2198a98673 | 74 | else{ |
jebradshaw | 0:ff2198a98673 | 75 | if(dc > 0.0){ |
jebradshaw | 0:ff2198a98673 | 76 | mot2_ph2 = 0; |
jebradshaw | 0:ff2198a98673 | 77 | mot2_ph1 = 1; |
jebradshaw | 0:ff2198a98673 | 78 | mot_en2 = dc; |
jebradshaw | 0:ff2198a98673 | 79 | } |
jebradshaw | 0:ff2198a98673 | 80 | else if(dc < -0.0){ |
jebradshaw | 0:ff2198a98673 | 81 | mot2_ph1 = 0; |
jebradshaw | 0:ff2198a98673 | 82 | mot2_ph2 = 1; |
jebradshaw | 0:ff2198a98673 | 83 | mot_en2 = abs(dc); |
jebradshaw | 0:ff2198a98673 | 84 | } |
jebradshaw | 0:ff2198a98673 | 85 | else{ |
jebradshaw | 0:ff2198a98673 | 86 | mot2_ph1 = 0; |
jebradshaw | 0:ff2198a98673 | 87 | mot2_ph2 = 0; |
jebradshaw | 0:ff2198a98673 | 88 | mot_en2 = 0.0; |
jebradshaw | 0:ff2198a98673 | 89 | } |
jebradshaw | 0:ff2198a98673 | 90 | } |
jebradshaw | 0:ff2198a98673 | 91 | } |
jebradshaw | 0:ff2198a98673 | 92 | |
jebradshaw | 0:ff2198a98673 | 93 | void mot1_thread(void const *args){ |
jebradshaw | 0:ff2198a98673 | 94 | while(1){ |
jebradshaw | 0:ff2198a98673 | 95 | // Run PWM and servo command signals |
jebradshaw | 0:ff2198a98673 | 96 | for(float cycle=0;cycle<2.0*PI;cycle+=.078){ |
jebradshaw | 0:ff2198a98673 | 97 | sv1 = 1500 + (int)(sin(cycle)*500); |
jebradshaw | 0:ff2198a98673 | 98 | sv2 = 1500 + (int)(cos(cycle)*500); |
jebradshaw | 0:ff2198a98673 | 99 | sv3 = 1500 + (int)(sin(cycle)*500); |
jebradshaw | 0:ff2198a98673 | 100 | sv4 = 1500 + (int)(sin(cycle)*500); |
jebradshaw | 0:ff2198a98673 | 101 | |
jebradshaw | 0:ff2198a98673 | 102 | mot_control(1, .85*sin(cycle)); |
jebradshaw | 0:ff2198a98673 | 103 | mot_control(2, .85*cos(cycle)); |
jebradshaw | 0:ff2198a98673 | 104 | |
jebradshaw | 0:ff2198a98673 | 105 | pc.printf("cycle = %.2f \r", cycle); |
jebradshaw | 0:ff2198a98673 | 106 | Thread::wait(10); |
jebradshaw | 0:ff2198a98673 | 107 | } |
jebradshaw | 0:ff2198a98673 | 108 | }//while(1) |
jebradshaw | 0:ff2198a98673 | 109 | } |
jebradshaw | 0:ff2198a98673 | 110 | |
jebradshaw | 0:ff2198a98673 | 111 | void digOut_thread(void const *args){ |
jebradshaw | 0:ff2198a98673 | 112 | //toggle bus I/O for testing |
jebradshaw | 0:ff2198a98673 | 113 | while(1){ |
jebradshaw | 0:ff2198a98673 | 114 | bus_J2 = 0x1f; |
jebradshaw | 0:ff2198a98673 | 115 | Thread::wait(500); |
jebradshaw | 0:ff2198a98673 | 116 | bus_J2 = 0xAA; |
jebradshaw | 0:ff2198a98673 | 117 | Thread::wait(100); |
jebradshaw | 0:ff2198a98673 | 118 | bus_J2 = 0x55; |
jebradshaw | 0:ff2198a98673 | 119 | Thread::wait(100); |
jebradshaw | 0:ff2198a98673 | 120 | bus_J2 = 0xAA; |
jebradshaw | 0:ff2198a98673 | 121 | Thread::wait(100); |
jebradshaw | 0:ff2198a98673 | 122 | bus_J2 = 0x55; |
jebradshaw | 0:ff2198a98673 | 123 | Thread::wait(100); |
jebradshaw | 0:ff2198a98673 | 124 | bus_J2 = 0; //all off |
jebradshaw | 0:ff2198a98673 | 125 | Thread::wait(500); |
jebradshaw | 0:ff2198a98673 | 126 | } |
jebradshaw | 0:ff2198a98673 | 127 | } |
jebradshaw | 0:ff2198a98673 | 128 | |
jebradshaw | 0:ff2198a98673 | 129 | void led1_flash(void const *args){ |
jebradshaw | 0:ff2198a98673 | 130 | while(1){ |
jebradshaw | 0:ff2198a98673 | 131 | led1 = !led1; |
jebradshaw | 0:ff2198a98673 | 132 | Thread::wait(500); |
jebradshaw | 0:ff2198a98673 | 133 | } |
jebradshaw | 0:ff2198a98673 | 134 | } |
jebradshaw | 0:ff2198a98673 | 135 | //------------ Main ------------------------------ |
jebradshaw | 0:ff2198a98673 | 136 | int main() { |
jebradshaw | 0:ff2198a98673 | 137 | float analogVal[6]; |
jebradshaw | 0:ff2198a98673 | 138 | |
jebradshaw | 0:ff2198a98673 | 139 | Thread t1(mot1_thread); |
jebradshaw | 0:ff2198a98673 | 140 | Thread t2(digOut_thread); |
jebradshaw | 0:ff2198a98673 | 141 | Thread t3(led1_flash); |
jebradshaw | 0:ff2198a98673 | 142 | |
jebradshaw | 0:ff2198a98673 | 143 | pc.baud(9600); |
jebradshaw | 0:ff2198a98673 | 144 | DB9_female.baud(9600); |
jebradshaw | 0:ff2198a98673 | 145 | DB9_male.baud(9600); |
jebradshaw | 0:ff2198a98673 | 146 | mot_en1.period_us(20); |
jebradshaw | 0:ff2198a98673 | 147 | mot_en2.period_us(20); |
jebradshaw | 0:ff2198a98673 | 148 | |
jebradshaw | 0:ff2198a98673 | 149 | while(1) { |
jebradshaw | 0:ff2198a98673 | 150 | analogVal[0] = J3_P16; |
jebradshaw | 0:ff2198a98673 | 151 | analogVal[1] = J3_P17; |
jebradshaw | 0:ff2198a98673 | 152 | analogVal[2] = J3_P18; |
jebradshaw | 0:ff2198a98673 | 153 | analogVal[3] = J3_P19; |
jebradshaw | 0:ff2198a98673 | 154 | analogVal[4] = J3_P20; |
jebradshaw | 0:ff2198a98673 | 155 | pc.printf("%.2f %.2f %.2f %.2f %.2f \r\n", analogVal[0],analogVal[1],analogVal[2],analogVal[3],analogVal[4]); |
jebradshaw | 0:ff2198a98673 | 156 | 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]); |
jebradshaw | 0:ff2198a98673 | 157 | |
jebradshaw | 0:ff2198a98673 | 158 | pc.printf("\r\n"); |
jebradshaw | 0:ff2198a98673 | 159 | } |
jebradshaw | 0:ff2198a98673 | 160 | } |