Joseph Bradshaw / Mbed 2 deprecated mbed_ES20X_Tester

Test Program for the ES20X mbed board

Dependencies:   ServoOut mbed-rtos mbed

Fork of mbed_ES20X_Thread_Test by Joseph Bradshaw

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?

UserRevisionLine numberNew 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 }