Diff: main.cpp

Revision:

0:95217092dfba

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri May 27 21:11:01 2016 +0000
@@ -0,0 +1,296 @@
+/*--------------------------------------------------------------------------------*/
+/*  Plotter Demo Code for K64F (Motors, Ethernet Server)                          */
+/*--------------------------------------------------------------------------------*/
+                                                                                        
+/*--COMPANY-----AUTHOR------DATE------------REVISION----NOTES---------------------*/
+/*  NXP         mareikeFSL  2016.02.05      rev 1.0     initial                   */
+/*  NXP         mareikeFSL  2016.03.02      rev 1.1     added feedback / enable   */
+/*                                                                                */
+/*--------------------------------------------------------------------------------*/
+
+/*--PROGRAM FLOW------------------------------------------------------------------*/
+/*  MAIN                                                                          */
+/*  ->                                                                            */
+/*--------------------------------------------------------------------------------*/
+
+/*--HARDWARE----------------------------------------------------------------------*/
+/*  FRDM-K64F #1        REV: 700-28163 REV C, SCH-28163 REV E                     */
+/*  FRDM-34931S-EVB #1  REV: 700-????? REV A, SCH-????? REV A                     */
+/*  FRDM-34931S-EVB #2  REV: 700-????? REV A, SCH-????? REV A                     */
+/*  FRDM-34931S-EVB #1  JP1/JP2/JP3/JP4/JP5/JP6JP7 (1-2)                           */
+/*  FRDM-34931S-EVB #2  JP1/JP6/JP7 (---), JP2/JP3/JP4/JP5 (1-2)                   */
+/*  Ethernet cable from FRDM-K64F (#1) to FRDM-K64F (#2)                          */
+/*--------------------------------------------------------------------------------*/
+
+/*--GPIO--------------------------------------------------------------------------*/
+/*  IO13    (M1_D1)        =   PTC17    (J1/4)                                    */
+/*  IO8     (M1_EN/D2_B)   =   PTE25    (J2/18)                                   */
+/*  IO12    (M2_D1)        =   PTC17    (J1/4)                                    */
+/*  IO9     (M2_EN/D2_B)   =   PTE25    (J2/18)                                   */
+/*--------------------------------------------------------------------------------*/
+/*--PWM---------------------------------------------------------------------------*/
+/*  PWM1    (M1_IN1)       =   PTA2    (J1/12)                                    */
+/*  PWM2    (M1_IN2)       =   PTC2    (J1/14)                                    */
+/*  PWM5    (M2_IN1)       =   PTD2    (J2/8)                                     */
+/*  PWM4    (M2_IN2)       =   PTD0    (J2/6)                                     */
+/*--------------------------------------------------------------------------------*/
+/*--ANALOG------------------------------------------------------------------------*/
+/*  AN0     (M1_FB)        =   PTB2    (J4/2)                                     */
+/*  AN0     (M2_FB)        =   PTB3    (J4/4)                                     */
+/*--------------------------------------------------------------------------------*/
+
+
+/*--INCLUDES----------------------------------------------------------------------*/
+#include "mbed.h"
+#include "EthernetInterface.h"
+
+
+/*--DEFINES-----------------------------------------------------------------------*/
+#define CODE_MASK   0x03
+#define PWM_MASK    0x3F
+#define X_STOP      0xFA
+#define Y_STOP      0xFB
+
+
+/*--CONSTANTS---------------------------------------------------------------------*/
+const int PORT = 7;
+static const char* SERVER_IP = "192.168.1.101"; //IP of motor board
+static const char* MASK = "255.255.255.0";      //mask
+static const char* GATEWAY = "192.168.1.1";     //gateway
+
+
+/*--INITS-------------------------------------------------------------------------*/
+DigitalOut red(LED_RED);        //debug led
+DigitalOut green(LED_GREEN);    //debug led
+DigitalOut blue(LED_BLUE);      //debug led
+
+EthernetInterface eth;          //create ethernet
+UDPSocket server;               //creat server
+Endpoint client;                //create endpoint
+
+DigitalOut m1_d1(PTC17);
+DigitalOut m1_en_d2_b(PTE25);
+
+/* same as m1
+DigitalOut m2_d1(PTC17);
+DigitalOut m2_en_d2_b(PTE25);
+*/
+
+PwmOut m1_in1(PTA2);
+PwmOut m1_in2(PTC2);
+PwmOut m2_in1(PTD2);
+PwmOut m2_in2(PTD0);
+
+AnalogIn m1_fb(PTB2);
+AnalogIn m2_fb(PTB3);
+
+Serial pc(USBTX, USBRX);        //create PC interface
+
+
+/*--VARIABLES---------------------------------------------------------------------*/
+int n;                          //size of received message
+char data[1] = {0};             //sample receive/send buffer
+int pwm = 0;                    //duty cycle
+int m1_in1_previous_pwm = 0;    //previous state of motor 1 input 1
+int m1_in2_previous_pwm = 0;    //previous state of motor 1 input 2
+int m2_in1_previous_pwm = 0;    //previous state of motor 2 input 1
+int m2_in2_previous_pwm = 0;    //previous state of motor 2 input 2
+char code = 0;                  //motor/input code
+float input = 0;                //pwm code
+char toggle = 1;
+int i = 0;
+uint16_t CurrFB_m1 = 0; 
+uint16_t CFBAvg_m1 = 0;
+uint16_t CurrFB_m2 = 0; 
+uint16_t CFBAvg_m2 = 0;
+
+/*--FUNCTION DECLARATIONS---------------------------------------------------------*/
+void init_usb(void);    //initializes pc.printf if required
+void init_eth(void);    //initializes ethernet
+void init_mot(void);    //initializes motor boards
+void receive(void);     //receives ethernet packet
+void decode(void);      //decodes ethernet packet
+void check_FB(void);
+int main(void);         //main
+
+
+/*--FUNCTION DEFINITIONS----------------------------------------------------------*/
+
+/***********************************************************************INIT_USB***/
+void init_usb(void)
+{
+    pc.baud(9600);  //baud
+    
+}   //end init_usb()
+
+/***********************************************************************INIT_ETH***/
+void init_eth(void)
+{
+    eth.init(SERVER_IP, MASK, GATEWAY); //set up IP
+    eth.connect();                      //connect ethernet    
+    server.bind(PORT);                  //bind server
+
+}   //end init_eth()
+
+/***********************************************************************INIT_MOT***/
+void init_mot(void)
+{
+    m1_d1 = 0;      //motor 1
+    m1_en_d2_b = 1; //motor 1
+    
+    /* same as m1
+    m2_d1 = 1;      //motor 2
+    m2_en_d2_b = 0; //motor 2
+    */
+    
+}   //end init_mot()
+
+/************************************************************************RECEIVE***/
+void receive(void)
+{
+    if(toggle)
+    {
+        data[0] = X_STOP;
+        toggle = 0;
+    }
+    else
+    {
+        data[0] = Y_STOP;
+        toggle = 1;
+    }
+    
+    n = server.receiveFrom(client, data, sizeof(data)); //receive from client
+    data[n] = '\0';                                     //add \0 to end of message
+    //pc.printf("data %x\r\n", data[0]);
+    decode();                                           //decode received packet
+
+}   //end receive()
+
+/*************************************************************************DECODE***/
+void decode(void)
+{
+    code = (data[0] >> 6) & CODE_MASK;
+    pwm = data[0] & PWM_MASK;
+    
+    if(data[0] == X_STOP)
+    {
+        m1_in1.write(0);
+        m1_in2.write(0);
+        m1_in1_previous_pwm = 0;
+        m1_in2_previous_pwm = 0;
+    }
+    else if(data[0] == Y_STOP)
+    {
+        m2_in1.write(0);
+        m2_in2.write(0);
+        m2_in1_previous_pwm = 0;
+        m2_in2_previous_pwm = 0;
+    }
+    else
+    {
+        switch(code)
+        {
+            case 0:     //BOARD1_PWMA
+                if(pwm != m1_in1_previous_pwm)
+                {
+                    m1_in1_previous_pwm = pwm;
+                    m1_in2.write(0);
+                    input = (float)(pwm*0.02);
+                    m1_in1.write(input);
+                    //pc.printf("Right @ %f\r\n", input);
+                }
+                break;
+            case 1:     //BOARD1_PWMB
+                if(pwm != m1_in2_previous_pwm)
+                {
+                    m1_in2_previous_pwm = pwm;
+                    m1_in1.write(0);
+                    input = (float)(pwm*0.02);
+                    m1_in2.write(input);
+                    //pc.printf("Left @ %f\r\n", input);
+                }
+                break;
+            case 2:     //BOARD2_PWMA
+                if(pwm != m2_in1_previous_pwm)
+                {
+                    m2_in1_previous_pwm = pwm;
+                    m2_in2.write(0);
+                    input = (float)(pwm*0.02);
+                    m2_in1.write(input);
+                    //pc.printf("Backward @ %f\r\n", input);
+                }
+                break;
+            case 3:     //BOARD2_PWMB
+                if(pwm != m2_in2_previous_pwm)
+                {
+                    m2_in2_previous_pwm = pwm;
+                    m2_in1.write(0);
+                    input = (float)(pwm*0.02);
+                    m2_in2.write(input);
+                    //pc.printf("Forward @ %f\r\n", input);
+                }
+                break;
+            default:
+                //pc.printf("Stationary\r\n");
+                break;
+        }
+    }
+
+}   //end end_eth()
+
+/***********************************************************************CHECK FB***/
+void check_FB(void)
+{
+    CurrFB_m1 = m1_fb.read_u16();
+    CFBAvg_m1 = (CFBAvg_m1 >> 2) + (CurrFB_m1 >> 2);
+    
+    //CurrFB_m2 = m2_fb.read_u16();
+    //CFBAvg_m2 = (CFBAvg_m2 >> 2) + (CurrFB_m2 >> 2);
+
+}   //end check_FB()
+
+/**********************************************************************************/
+/***************************************************************************MAIN***/
+/**********************************************************************************/
+int main(void)
+{
+    red = 0;
+    green = 0;
+    blue = 0;
+    
+    init_usb();
+    init_eth(); //initialize the Ethernet connection
+    init_mot(); //initialize motor boards
+    
+    red = 1;
+    green = 0;  //server = green
+    blue = 1;
+    
+    m1_in1.period_ms(100);    //board 1, blue (A, right)
+    m1_in2.period_ms(100);    //board 1, purple (B, left)
+    m2_in1.period_ms(100);    //board 2, blue (A, backward)
+    m2_in2.period_ms(100);    //board 2, purple (B, forward)
+    
+    m1_in1.write(0);    //board 1, blue (A, right)
+    m1_in2.write(0);    //board 1, purple (B, left)
+    m2_in1.write(0);    //board 2, blue (A, backward)
+    m2_in2.write(0);    //board 2, purple (B, forward)
+    
+    while (true)
+    {
+        check_FB();
+        //pc.printf("0x%x\r\n", CFBAvg_m1);
+        //wait_ms(1);
+        if((CFBAvg_m1 > 0x500))  //stop
+        {
+            m1_in1.write(0);    //board 1, blue (A, right)
+            m1_in2.write(0);    //board 1, purple (B, left)
+            m2_in1.write(0);    //board 2, blue (A, backward)
+            m2_in2.write(0);    //board 2, purple (B, forward)
+            wait_ms(500);
+        }
+       
+        receive();
+    }
+
+}   //end main()
\ No newline at end of file