NXP
NXP Lego plotter demo code using FRDM-34931S-EVB, FRDM-STBC-AGM01, and FRDM-K64F (motor driver/server side).
Dependencies: EthernetInterface mbed-rtos mbed
Fork of
DEMO_plotter_mot-server
by 
Freescale
Revision 0:95217092dfba, committed 2016-05-27
- Comitter:
- mareikeFSL
- Date:
- Fri May 27 21:11:01 2016 +0000
- Commit message:
- Initial publication
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EthernetInterface.lib Fri May 27 21:11:01 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/EthernetInterface/#2fc406e2553f
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Fri May 27 21:11:01 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#c825593ece39
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri May 27 21:11:01 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/252557024ec3 \ No newline at end of file