NXP / Mbed 2 deprecated DEMO_plotter_mot-server

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

main.cpp@0:95217092dfba, 2016-05-27 (annotated)

Committer:
mareikeFSL
Date:
Fri May 27 21:11:01 2016 +0000
Revision:
0:95217092dfba
Initial publication

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mareikeFSL 0:95217092dfba 1 /*--------------------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 2 /* Plotter Demo Code for K64F (Motors, Ethernet Server) */
mareikeFSL 0:95217092dfba 3 /*--------------------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 4
mareikeFSL 0:95217092dfba 5 /*--COMPANY-----AUTHOR------DATE------------REVISION----NOTES---------------------*/
mareikeFSL 0:95217092dfba 6 /* NXP mareikeFSL 2016.02.05 rev 1.0 initial */
mareikeFSL 0:95217092dfba 7 /* NXP mareikeFSL 2016.03.02 rev 1.1 added feedback / enable */
mareikeFSL 0:95217092dfba 8 /* */
mareikeFSL 0:95217092dfba 9 /*--------------------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 10
mareikeFSL 0:95217092dfba 11 /*--PROGRAM FLOW------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 12 /* MAIN */
mareikeFSL 0:95217092dfba 13 /* -> */
mareikeFSL 0:95217092dfba 14 /*--------------------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 15
mareikeFSL 0:95217092dfba 16 /*--HARDWARE----------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 17 /* FRDM-K64F #1 REV: 700-28163 REV C, SCH-28163 REV E */
mareikeFSL 0:95217092dfba 18 /* FRDM-34931S-EVB #1 REV: 700-????? REV A, SCH-????? REV A */
mareikeFSL 0:95217092dfba 19 /* FRDM-34931S-EVB #2 REV: 700-????? REV A, SCH-????? REV A */
mareikeFSL 0:95217092dfba 20 /* FRDM-34931S-EVB #1 JP1/JP2/JP3/JP4/JP5/JP6JP7 (1-2) */
mareikeFSL 0:95217092dfba 21 /* FRDM-34931S-EVB #2 JP1/JP6/JP7 (---), JP2/JP3/JP4/JP5 (1-2) */
mareikeFSL 0:95217092dfba 22 /* Ethernet cable from FRDM-K64F (#1) to FRDM-K64F (#2) */
mareikeFSL 0:95217092dfba 23 /*--------------------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 24
mareikeFSL 0:95217092dfba 25 /*--GPIO--------------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 26 /* IO13 (M1_D1) = PTC17 (J1/4) */
mareikeFSL 0:95217092dfba 27 /* IO8 (M1_EN/D2_B) = PTE25 (J2/18) */
mareikeFSL 0:95217092dfba 28 /* IO12 (M2_D1) = PTC17 (J1/4) */
mareikeFSL 0:95217092dfba 29 /* IO9 (M2_EN/D2_B) = PTE25 (J2/18) */
mareikeFSL 0:95217092dfba 30 /*--------------------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 31 /*--PWM---------------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 32 /* PWM1 (M1_IN1) = PTA2 (J1/12) */
mareikeFSL 0:95217092dfba 33 /* PWM2 (M1_IN2) = PTC2 (J1/14) */
mareikeFSL 0:95217092dfba 34 /* PWM5 (M2_IN1) = PTD2 (J2/8) */
mareikeFSL 0:95217092dfba 35 /* PWM4 (M2_IN2) = PTD0 (J2/6) */
mareikeFSL 0:95217092dfba 36 /*--------------------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 37 /*--ANALOG------------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 38 /* AN0 (M1_FB) = PTB2 (J4/2) */
mareikeFSL 0:95217092dfba 39 /* AN0 (M2_FB) = PTB3 (J4/4) */
mareikeFSL 0:95217092dfba 40 /*--------------------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 41
mareikeFSL 0:95217092dfba 42
mareikeFSL 0:95217092dfba 43 /*--INCLUDES----------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 44 #include "mbed.h"
mareikeFSL 0:95217092dfba 45 #include "EthernetInterface.h"
mareikeFSL 0:95217092dfba 46
mareikeFSL 0:95217092dfba 47
mareikeFSL 0:95217092dfba 48 /*--DEFINES-----------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 49 #define CODE_MASK 0x03
mareikeFSL 0:95217092dfba 50 #define PWM_MASK 0x3F
mareikeFSL 0:95217092dfba 51 #define X_STOP 0xFA
mareikeFSL 0:95217092dfba 52 #define Y_STOP 0xFB
mareikeFSL 0:95217092dfba 53
mareikeFSL 0:95217092dfba 54
mareikeFSL 0:95217092dfba 55 /*--CONSTANTS---------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 56 const int PORT = 7;
mareikeFSL 0:95217092dfba 57 static const char* SERVER_IP = "192.168.1.101"; //IP of motor board
mareikeFSL 0:95217092dfba 58 static const char* MASK = "255.255.255.0"; //mask
mareikeFSL 0:95217092dfba 59 static const char* GATEWAY = "192.168.1.1"; //gateway
mareikeFSL 0:95217092dfba 60
mareikeFSL 0:95217092dfba 61
mareikeFSL 0:95217092dfba 62 /*--INITS-------------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 63 DigitalOut red(LED_RED); //debug led
mareikeFSL 0:95217092dfba 64 DigitalOut green(LED_GREEN); //debug led
mareikeFSL 0:95217092dfba 65 DigitalOut blue(LED_BLUE); //debug led
mareikeFSL 0:95217092dfba 66
mareikeFSL 0:95217092dfba 67 EthernetInterface eth; //create ethernet
mareikeFSL 0:95217092dfba 68 UDPSocket server; //creat server
mareikeFSL 0:95217092dfba 69 Endpoint client; //create endpoint
mareikeFSL 0:95217092dfba 70
mareikeFSL 0:95217092dfba 71 DigitalOut m1_d1(PTC17);
mareikeFSL 0:95217092dfba 72 DigitalOut m1_en_d2_b(PTE25);
mareikeFSL 0:95217092dfba 73
mareikeFSL 0:95217092dfba 74 /* same as m1
mareikeFSL 0:95217092dfba 75 DigitalOut m2_d1(PTC17);
mareikeFSL 0:95217092dfba 76 DigitalOut m2_en_d2_b(PTE25);
mareikeFSL 0:95217092dfba 77 */
mareikeFSL 0:95217092dfba 78
mareikeFSL 0:95217092dfba 79 PwmOut m1_in1(PTA2);
mareikeFSL 0:95217092dfba 80 PwmOut m1_in2(PTC2);
mareikeFSL 0:95217092dfba 81 PwmOut m2_in1(PTD2);
mareikeFSL 0:95217092dfba 82 PwmOut m2_in2(PTD0);
mareikeFSL 0:95217092dfba 83
mareikeFSL 0:95217092dfba 84 AnalogIn m1_fb(PTB2);
mareikeFSL 0:95217092dfba 85 AnalogIn m2_fb(PTB3);
mareikeFSL 0:95217092dfba 86
mareikeFSL 0:95217092dfba 87 Serial pc(USBTX, USBRX); //create PC interface
mareikeFSL 0:95217092dfba 88
mareikeFSL 0:95217092dfba 89
mareikeFSL 0:95217092dfba 90 /*--VARIABLES---------------------------------------------------------------------*/
mareikeFSL 0:95217092dfba 91 int n; //size of received message
mareikeFSL 0:95217092dfba 92 char data[1] = {0}; //sample receive/send buffer
mareikeFSL 0:95217092dfba 93 int pwm = 0; //duty cycle
mareikeFSL 0:95217092dfba 94 int m1_in1_previous_pwm = 0; //previous state of motor 1 input 1
mareikeFSL 0:95217092dfba 95 int m1_in2_previous_pwm = 0; //previous state of motor 1 input 2
mareikeFSL 0:95217092dfba 96 int m2_in1_previous_pwm = 0; //previous state of motor 2 input 1
mareikeFSL 0:95217092dfba 97 int m2_in2_previous_pwm = 0; //previous state of motor 2 input 2
mareikeFSL 0:95217092dfba 98 char code = 0; //motor/input code
mareikeFSL 0:95217092dfba 99 float input = 0; //pwm code
mareikeFSL 0:95217092dfba 100 char toggle = 1;
mareikeFSL 0:95217092dfba 101 int i = 0;
mareikeFSL 0:95217092dfba 102 uint16_t CurrFB_m1 = 0;
mareikeFSL 0:95217092dfba 103 uint16_t CFBAvg_m1 = 0;
mareikeFSL 0:95217092dfba 104 uint16_t CurrFB_m2 = 0;
mareikeFSL 0:95217092dfba 105 uint16_t CFBAvg_m2 = 0;
mareikeFSL 0:95217092dfba 106
mareikeFSL 0:95217092dfba 107 /*--FUNCTION DECLARATIONS---------------------------------------------------------*/
mareikeFSL 0:95217092dfba 108 void init_usb(void); //initializes pc.printf if required
mareikeFSL 0:95217092dfba 109 void init_eth(void); //initializes ethernet
mareikeFSL 0:95217092dfba 110 void init_mot(void); //initializes motor boards
mareikeFSL 0:95217092dfba 111 void receive(void); //receives ethernet packet
mareikeFSL 0:95217092dfba 112 void decode(void); //decodes ethernet packet
mareikeFSL 0:95217092dfba 113 void check_FB(void);
mareikeFSL 0:95217092dfba 114 int main(void); //main
mareikeFSL 0:95217092dfba 115
mareikeFSL 0:95217092dfba 116
mareikeFSL 0:95217092dfba 117 /*--FUNCTION DEFINITIONS----------------------------------------------------------*/
mareikeFSL 0:95217092dfba 118
mareikeFSL 0:95217092dfba 119 /***********************************************************************INIT_USB***/
mareikeFSL 0:95217092dfba 120 void init_usb(void)
mareikeFSL 0:95217092dfba 121 {
mareikeFSL 0:95217092dfba 122 pc.baud(9600); //baud
mareikeFSL 0:95217092dfba 123
mareikeFSL 0:95217092dfba 124 } //end init_usb()
mareikeFSL 0:95217092dfba 125
mareikeFSL 0:95217092dfba 126 /***********************************************************************INIT_ETH***/
mareikeFSL 0:95217092dfba 127 void init_eth(void)
mareikeFSL 0:95217092dfba 128 {
mareikeFSL 0:95217092dfba 129 eth.init(SERVER_IP, MASK, GATEWAY); //set up IP
mareikeFSL 0:95217092dfba 130 eth.connect(); //connect ethernet
mareikeFSL 0:95217092dfba 131 server.bind(PORT); //bind server
mareikeFSL 0:95217092dfba 132
mareikeFSL 0:95217092dfba 133 } //end init_eth()
mareikeFSL 0:95217092dfba 134
mareikeFSL 0:95217092dfba 135 /***********************************************************************INIT_MOT***/
mareikeFSL 0:95217092dfba 136 void init_mot(void)
mareikeFSL 0:95217092dfba 137 {
mareikeFSL 0:95217092dfba 138 m1_d1 = 0; //motor 1
mareikeFSL 0:95217092dfba 139 m1_en_d2_b = 1; //motor 1
mareikeFSL 0:95217092dfba 140
mareikeFSL 0:95217092dfba 141 /* same as m1
mareikeFSL 0:95217092dfba 142 m2_d1 = 1; //motor 2
mareikeFSL 0:95217092dfba 143 m2_en_d2_b = 0; //motor 2
mareikeFSL 0:95217092dfba 144 */
mareikeFSL 0:95217092dfba 145
mareikeFSL 0:95217092dfba 146 } //end init_mot()
mareikeFSL 0:95217092dfba 147
mareikeFSL 0:95217092dfba 148 /************************************************************************RECEIVE***/
mareikeFSL 0:95217092dfba 149 void receive(void)
mareikeFSL 0:95217092dfba 150 {
mareikeFSL 0:95217092dfba 151 if(toggle)
mareikeFSL 0:95217092dfba 152 {
mareikeFSL 0:95217092dfba 153 data[0] = X_STOP;
mareikeFSL 0:95217092dfba 154 toggle = 0;
mareikeFSL 0:95217092dfba 155 }
mareikeFSL 0:95217092dfba 156 else
mareikeFSL 0:95217092dfba 157 {
mareikeFSL 0:95217092dfba 158 data[0] = Y_STOP;
mareikeFSL 0:95217092dfba 159 toggle = 1;
mareikeFSL 0:95217092dfba 160 }
mareikeFSL 0:95217092dfba 161
mareikeFSL 0:95217092dfba 162 n = server.receiveFrom(client, data, sizeof(data)); //receive from client
mareikeFSL 0:95217092dfba 163 data[n] = '\0'; //add \0 to end of message
mareikeFSL 0:95217092dfba 164 //pc.printf("data %x\r\n", data[0]);
mareikeFSL 0:95217092dfba 165 decode(); //decode received packet
mareikeFSL 0:95217092dfba 166
mareikeFSL 0:95217092dfba 167 } //end receive()
mareikeFSL 0:95217092dfba 168
mareikeFSL 0:95217092dfba 169 /*************************************************************************DECODE***/
mareikeFSL 0:95217092dfba 170 void decode(void)
mareikeFSL 0:95217092dfba 171 {
mareikeFSL 0:95217092dfba 172 code = (data[0] >> 6) & CODE_MASK;
mareikeFSL 0:95217092dfba 173 pwm = data[0] & PWM_MASK;
mareikeFSL 0:95217092dfba 174
mareikeFSL 0:95217092dfba 175 if(data[0] == X_STOP)
mareikeFSL 0:95217092dfba 176 {
mareikeFSL 0:95217092dfba 177 m1_in1.write(0);
mareikeFSL 0:95217092dfba 178 m1_in2.write(0);
mareikeFSL 0:95217092dfba 179 m1_in1_previous_pwm = 0;
mareikeFSL 0:95217092dfba 180 m1_in2_previous_pwm = 0;
mareikeFSL 0:95217092dfba 181 }
mareikeFSL 0:95217092dfba 182 else if(data[0] == Y_STOP)
mareikeFSL 0:95217092dfba 183 {
mareikeFSL 0:95217092dfba 184 m2_in1.write(0);
mareikeFSL 0:95217092dfba 185 m2_in2.write(0);
mareikeFSL 0:95217092dfba 186 m2_in1_previous_pwm = 0;
mareikeFSL 0:95217092dfba 187 m2_in2_previous_pwm = 0;
mareikeFSL 0:95217092dfba 188 }
mareikeFSL 0:95217092dfba 189 else
mareikeFSL 0:95217092dfba 190 {
mareikeFSL 0:95217092dfba 191 switch(code)
mareikeFSL 0:95217092dfba 192 {
mareikeFSL 0:95217092dfba 193 case 0: //BOARD1_PWMA
mareikeFSL 0:95217092dfba 194 if(pwm != m1_in1_previous_pwm)
mareikeFSL 0:95217092dfba 195 {
mareikeFSL 0:95217092dfba 196 m1_in1_previous_pwm = pwm;
mareikeFSL 0:95217092dfba 197 m1_in2.write(0);
mareikeFSL 0:95217092dfba 198 input = (float)(pwm*0.02);
mareikeFSL 0:95217092dfba 199 m1_in1.write(input);
mareikeFSL 0:95217092dfba 200 //pc.printf("Right @ %f\r\n", input);
mareikeFSL 0:95217092dfba 201 }
mareikeFSL 0:95217092dfba 202 break;
mareikeFSL 0:95217092dfba 203 case 1: //BOARD1_PWMB
mareikeFSL 0:95217092dfba 204 if(pwm != m1_in2_previous_pwm)
mareikeFSL 0:95217092dfba 205 {
mareikeFSL 0:95217092dfba 206 m1_in2_previous_pwm = pwm;
mareikeFSL 0:95217092dfba 207 m1_in1.write(0);
mareikeFSL 0:95217092dfba 208 input = (float)(pwm*0.02);
mareikeFSL 0:95217092dfba 209 m1_in2.write(input);
mareikeFSL 0:95217092dfba 210 //pc.printf("Left @ %f\r\n", input);
mareikeFSL 0:95217092dfba 211 }
mareikeFSL 0:95217092dfba 212 break;
mareikeFSL 0:95217092dfba 213 case 2: //BOARD2_PWMA
mareikeFSL 0:95217092dfba 214 if(pwm != m2_in1_previous_pwm)
mareikeFSL 0:95217092dfba 215 {
mareikeFSL 0:95217092dfba 216 m2_in1_previous_pwm = pwm;
mareikeFSL 0:95217092dfba 217 m2_in2.write(0);
mareikeFSL 0:95217092dfba 218 input = (float)(pwm*0.02);
mareikeFSL 0:95217092dfba 219 m2_in1.write(input);
mareikeFSL 0:95217092dfba 220 //pc.printf("Backward @ %f\r\n", input);
mareikeFSL 0:95217092dfba 221 }
mareikeFSL 0:95217092dfba 222 break;
mareikeFSL 0:95217092dfba 223 case 3: //BOARD2_PWMB
mareikeFSL 0:95217092dfba 224 if(pwm != m2_in2_previous_pwm)
mareikeFSL 0:95217092dfba 225 {
mareikeFSL 0:95217092dfba 226 m2_in2_previous_pwm = pwm;
mareikeFSL 0:95217092dfba 227 m2_in1.write(0);
mareikeFSL 0:95217092dfba 228 input = (float)(pwm*0.02);
mareikeFSL 0:95217092dfba 229 m2_in2.write(input);
mareikeFSL 0:95217092dfba 230 //pc.printf("Forward @ %f\r\n", input);
mareikeFSL 0:95217092dfba 231 }
mareikeFSL 0:95217092dfba 232 break;
mareikeFSL 0:95217092dfba 233 default:
mareikeFSL 0:95217092dfba 234 //pc.printf("Stationary\r\n");
mareikeFSL 0:95217092dfba 235 break;
mareikeFSL 0:95217092dfba 236 }
mareikeFSL 0:95217092dfba 237 }
mareikeFSL 0:95217092dfba 238
mareikeFSL 0:95217092dfba 239 } //end end_eth()
mareikeFSL 0:95217092dfba 240
mareikeFSL 0:95217092dfba 241 /***********************************************************************CHECK FB***/
mareikeFSL 0:95217092dfba 242 void check_FB(void)
mareikeFSL 0:95217092dfba 243 {
mareikeFSL 0:95217092dfba 244 CurrFB_m1 = m1_fb.read_u16();
mareikeFSL 0:95217092dfba 245 CFBAvg_m1 = (CFBAvg_m1 >> 2) + (CurrFB_m1 >> 2);
mareikeFSL 0:95217092dfba 246
mareikeFSL 0:95217092dfba 247 //CurrFB_m2 = m2_fb.read_u16();
mareikeFSL 0:95217092dfba 248 //CFBAvg_m2 = (CFBAvg_m2 >> 2) + (CurrFB_m2 >> 2);
mareikeFSL 0:95217092dfba 249
mareikeFSL 0:95217092dfba 250 } //end check_FB()
mareikeFSL 0:95217092dfba 251
mareikeFSL 0:95217092dfba 252 /**********************************************************************************/
mareikeFSL 0:95217092dfba 253 /***************************************************************************MAIN***/
mareikeFSL 0:95217092dfba 254 /**********************************************************************************/
mareikeFSL 0:95217092dfba 255 int main(void)
mareikeFSL 0:95217092dfba 256 {
mareikeFSL 0:95217092dfba 257 red = 0;
mareikeFSL 0:95217092dfba 258 green = 0;
mareikeFSL 0:95217092dfba 259 blue = 0;
mareikeFSL 0:95217092dfba 260
mareikeFSL 0:95217092dfba 261 init_usb();
mareikeFSL 0:95217092dfba 262 init_eth(); //initialize the Ethernet connection
mareikeFSL 0:95217092dfba 263 init_mot(); //initialize motor boards
mareikeFSL 0:95217092dfba 264
mareikeFSL 0:95217092dfba 265 red = 1;
mareikeFSL 0:95217092dfba 266 green = 0; //server = green
mareikeFSL 0:95217092dfba 267 blue = 1;
mareikeFSL 0:95217092dfba 268
mareikeFSL 0:95217092dfba 269 m1_in1.period_ms(100); //board 1, blue (A, right)
mareikeFSL 0:95217092dfba 270 m1_in2.period_ms(100); //board 1, purple (B, left)
mareikeFSL 0:95217092dfba 271 m2_in1.period_ms(100); //board 2, blue (A, backward)
mareikeFSL 0:95217092dfba 272 m2_in2.period_ms(100); //board 2, purple (B, forward)
mareikeFSL 0:95217092dfba 273
mareikeFSL 0:95217092dfba 274 m1_in1.write(0); //board 1, blue (A, right)
mareikeFSL 0:95217092dfba 275 m1_in2.write(0); //board 1, purple (B, left)
mareikeFSL 0:95217092dfba 276 m2_in1.write(0); //board 2, blue (A, backward)
mareikeFSL 0:95217092dfba 277 m2_in2.write(0); //board 2, purple (B, forward)
mareikeFSL 0:95217092dfba 278
mareikeFSL 0:95217092dfba 279 while (true)
mareikeFSL 0:95217092dfba 280 {
mareikeFSL 0:95217092dfba 281 check_FB();
mareikeFSL 0:95217092dfba 282 //pc.printf("0x%x\r\n", CFBAvg_m1);
mareikeFSL 0:95217092dfba 283 //wait_ms(1);
mareikeFSL 0:95217092dfba 284 if((CFBAvg_m1 > 0x500)) //stop
mareikeFSL 0:95217092dfba 285 {
mareikeFSL 0:95217092dfba 286 m1_in1.write(0); //board 1, blue (A, right)
mareikeFSL 0:95217092dfba 287 m1_in2.write(0); //board 1, purple (B, left)
mareikeFSL 0:95217092dfba 288 m2_in1.write(0); //board 2, blue (A, backward)
mareikeFSL 0:95217092dfba 289 m2_in2.write(0); //board 2, purple (B, forward)
mareikeFSL 0:95217092dfba 290 wait_ms(500);
mareikeFSL 0:95217092dfba 291 }
mareikeFSL 0:95217092dfba 292
mareikeFSL 0:95217092dfba 293 receive();
mareikeFSL 0:95217092dfba 294 }
mareikeFSL 0:95217092dfba 295
mareikeFSL 0:95217092dfba 296 } //end main()