Diff: main.cpp

Revision:

0:ff2198a98673

Child:

1:31677f9c113f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jan 08 16:53:32 2016 +0000
@@ -0,0 +1,160 @@
+// 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");
+    }
+}