Mateusz Jaskula / Mbed 2 deprecated KL25Z_TangoIO

KL25 driver for Tango Control System

Dependencies:   mbed

main.cpp@2:9fe6f1e273b4, 2014-08-28 (annotated)

Committer:
jskl
Date:
Thu Aug 28 07:50:06 2014 +0000
Revision:
2:9fe6f1e273b4
Parent:
1:f1ee978773d3 
Fixed bugs in JSON format replies

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jskl 0:5d27c333afa6 1
jskl 0:5d27c333afa6 2 #include "mbed.h"
jskl 0:5d27c333afa6 3 #include "WIZnetInterface.h"
jskl 0:5d27c333afa6 4 #include "GY80.h"
jskl 0:5d27c333afa6 5 #include "tsi_sensor.h"
jskl 0:5d27c333afa6 6 #include "main.h"
jskl 0:5d27c333afa6 7
jskl 0:5d27c333afa6 8
jskl 0:5d27c333afa6 9 int main() /// Main Function:
jskl 0:5d27c333afa6 10 {
jskl 0:5d27c333afa6 11
jskl 0:5d27c333afa6 12
jskl 0:5d27c333afa6 13 pc.attach(&SerialInterHandler, Serial::RxIrq); /// - configures serial port interrupts & choose handler
jskl 0:5d27c333afa6 14 menu_prop(); /// - shows 5 sec menu
jskl 0:5d27c333afa6 15 pc.attach(NULL, Serial::RxIrq); /// - disables serial port interrupts after 5s
jskl 0:5d27c333afa6 16 tick1.attach_us(&read_data, 10000); /// - reads data from sensors in 10ms intervals
jskl 0:5d27c333afa6 17 if(init_eth() == 1) { /// - initializes ethernet communication
jskl 0:5d27c333afa6 18 wait(0.001); // wait
jskl 0:5d27c333afa6 19 eth_comm(); /// - and tcp/ip server if no error occured
jskl 0:5d27c333afa6 20 } else { //
jskl 0:5d27c333afa6 21 return -1; /// - or returns an error
jskl 0:5d27c333afa6 22 } ///
jskl 0:5d27c333afa6 23 while(1); // infinit loop
jskl 0:5d27c333afa6 24 }
jskl 0:5d27c333afa6 25
jskl 0:5d27c333afa6 26 int init_eth()
jskl 0:5d27c333afa6 27 {
jskl 0:5d27c333afa6 28 /**
jskl 0:5d27c333afa6 29 * Initializes ethernet communication by configuring WIZ550io via SPI interface.
jskl 0:5d27c333afa6 30 * DHCP or Static IP configuration could be used.
jskl 0:5d27c333afa6 31 * Use of them is user configurable via serial port.
jskl 0:5d27c333afa6 32 */
jskl 0:5d27c333afa6 33 int ret; // error code variable
jskl 0:5d27c333afa6 34
jskl 0:5d27c333afa6 35 // MAC Adress setting:
jskl 0:5d27c333afa6 36 mac[0] = 0x00;
jskl 0:5d27c333afa6 37 mac[1] = 0x08;
jskl 0:5d27c333afa6 38 mac[2] = 0xDC;
jskl 0:5d27c333afa6 39 mac[3] = 0x1D;
jskl 0:5d27c333afa6 40 mac[4] = 0x2E;
jskl 0:5d27c333afa6 41 mac[5] = 0x74;
jskl 0:5d27c333afa6 42
jskl 0:5d27c333afa6 43 pc.printf("\r \n Ethernet Initialization \n");
jskl 0:5d27c333afa6 44
jskl 0:5d27c333afa6 45 if (USE_DHCP){
jskl 0:5d27c333afa6 46 ret = eth.init(mac); // Use DHCP
jskl 0:5d27c333afa6 47 }
jskl 0:5d27c333afa6 48 else{
jskl 0:5d27c333afa6 49 ret = eth.init(mac, IP_Addr, IP_Subnet, IP_Gateway); // static configuration
jskl 0:5d27c333afa6 50 }
jskl 0:5d27c333afa6 51 if (!ret) { // if error don't happen
jskl 0:5d27c333afa6 52 pc.printf("\r \n Initialized, MAC: %s\n", eth.getMACAddress());
jskl 0:5d27c333afa6 53 } else { // else show error code
jskl 0:5d27c333afa6 54 pc.printf("\r \n Error eth.init() - ret = %d\n", ret);
jskl 0:5d27c333afa6 55 return -1;
jskl 0:5d27c333afa6 56 }
jskl 0:5d27c333afa6 57
jskl 0:5d27c333afa6 58 ret = eth.connect();
jskl 0:5d27c333afa6 59 if (!ret) {
jskl 0:5d27c333afa6 60 pc.printf("\r \nIP: %s, MASK: %s, GW: %s\n",
jskl 0:5d27c333afa6 61 eth.getIPAddress(), eth.getNetworkMask(), eth.getGateway());
jskl 0:5d27c333afa6 62 } else {
jskl 0:5d27c333afa6 63 pc.printf("\r \n Error eth.connect() - ret = %d\n", ret);
jskl 0:5d27c333afa6 64 return -1;
jskl 0:5d27c333afa6 65 }
jskl 0:5d27c333afa6 66 return 1;
jskl 0:5d27c333afa6 67
jskl 0:5d27c333afa6 68 }
jskl 0:5d27c333afa6 69
jskl 0:5d27c333afa6 70 void eth_comm()
jskl 0:5d27c333afa6 71 {
jskl 0:5d27c333afa6 72 /**
jskl 0:5d27c333afa6 73 * Creates server and waits for client to connect.
jskl 0:5d27c333afa6 74 * Sends reveiced buffer with commands to comm_handler.
jskl 0:5d27c333afa6 75 * And sends reply created by comm_handler to client.
jskl 0:5d27c333afa6 76 */
jskl 0:5d27c333afa6 77 TCPSocketServer server; // TCP/IP server object
jskl 0:5d27c333afa6 78 server.bind(ECHO_SERVER_PORT); // binding listen port
jskl 0:5d27c333afa6 79 server.listen(); // start listening
jskl 0:5d27c333afa6 80
jskl 0:5d27c333afa6 81 while (true) { // infinit loop
jskl 0:5d27c333afa6 82 pc.printf("\n \r Wait for new connection...\n"); // pc info
jskl 0:5d27c333afa6 83 TCPSocketConnection client; // TCP/IP client object
jskl 0:5d27c333afa6 84 server.accept(client); // accept connection
jskl 2:9fe6f1e273b4 85 client.set_blocking(false, 4000); // no blocking connections, timeout after (1.5)s
jskl 0:5d27c333afa6 86 pc.printf("\n \r Connection from: %s\n", client.get_address()); // pc info
jskl 0:5d27c333afa6 87 char buffer[256]; // Data buffer
jskl 0:5d27c333afa6 88 while (true) { // loop
jskl 0:5d27c333afa6 89 for(int i =0; i < 256; i++) buffer[i] = '\0'; // clear buffer
jskl 0:5d27c333afa6 90 int n = client.receive(buffer, sizeof(buffer)); // receive data & save it to buffer
jskl 0:5d27c333afa6 91 if (n <= 0) break; // break if buffer is empty or error occured
jskl 0:5d27c333afa6 92 comm_handler(buffer); // send received command to communication handler
jskl 0:5d27c333afa6 93 n = strlen(buffer); // check buffer length
jskl 0:5d27c333afa6 94 pc.printf("\n \r BUFF: %s, strlen: %d, size: %d, n: %d", buffer, strlen(buffer), sizeof(buffer), n); // pc info, just for debug
jskl 0:5d27c333afa6 95 client.send(buffer, n); // send reply
jskl 0:5d27c333afa6 96 for(int i =0; i < 256; i++) buffer[i] = '\0'; // clear buffer
jskl 0:5d27c333afa6 97 if (n <= 0) break; // if error occured send break
jskl 0:5d27c333afa6 98 } //
jskl 0:5d27c333afa6 99 client.close(); // close connection
jskl 0:5d27c333afa6 100 } //
jskl 0:5d27c333afa6 101 }
jskl 0:5d27c333afa6 102
jskl 0:5d27c333afa6 103 void read_data()
jskl 0:5d27c333afa6 104 {
jskl 0:5d27c333afa6 105 /**
jskl 0:5d27c333afa6 106 * Reads sensor data in 10ms intervals.
jskl 0:5d27c333afa6 107 * And saves them.
jskl 0:5d27c333afa6 108 */
jskl 0:5d27c333afa6 109 sensor.Read_Magn(magn); // read magnitude
jskl 0:5d27c333afa6 110 sensor.Read_Accel(accel); // read acceleration
jskl 0:5d27c333afa6 111 sensor.Read_Gyro(gyro); // read gyroscope
jskl 0:5d27c333afa6 112 TSI_pos = tsi.readPercentage(); // read percentage position of touch sensor
jskl 2:9fe6f1e273b4 113 //double pitch = (atan2(-accel[1], accel[2]) * 180.0) / 3.14;
jskl 2:9fe6f1e273b4 114 //double roll = (atan2(accel[0], sqrt(accel[1]*[accel[1] + accel[2] * accel[2])) * 180.0) / 3.14;
jskl 2:9fe6f1e273b4 115 //pc.printf("roll: %f pitch: %f ,roll", pitch);
jskl 0:5d27c333afa6 116 }
jskl 0:5d27c333afa6 117
jskl 0:5d27c333afa6 118 void comm_handler(char* command)
jskl 0:5d27c333afa6 119 {
jskl 0:5d27c333afa6 120 /** This function gets data buffer with command
jskl 0:5d27c333afa6 121 * and prepares answer to be sent.
jskl 0:5d27c333afa6 122 */
jskl 0:5d27c333afa6 123
jskl 0:5d27c333afa6 124 pc.printf("\n \r %s \n \r", command); // send received command to pc (DEBUG)
jskl 0:5d27c333afa6 125 if(strcmp(command,"read_sensor")==0) { // data read from sensor
jskl 2:9fe6f1e273b4 126 sprintf(command, "{\"magnX\":%10f,\"magnY\":%10f,\"magnZ\":%10f,\"accelX\":%10f,\"accelY\":%10f,\"accelZ\":%10f,\"gyroX\":%10f,\"gyroY\":%10f,\"gyroZ\":%10f,\"TSIpos\":%10f}", magn[0], magn[1], magn[2], accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2], TSI_pos); // enter data to buffer
jskl 0:5d27c333afa6 127 wait(0.001); // wait
jskl 1:f1ee978773d3 128 pc.printf("\n \r sending sensor data! %s", command); // pc info (debug)
jskl 0:5d27c333afa6 129 } else if (strcmp(command,"check_ports")==0) { // check I/O ports current values
jskl 2:9fe6f1e273b4 130 sprintf(command, "{\"P1\":%d,\"P2\":%d,\"P3\":%d,\"P4\":%d,\"P5\":%d,\"P6\":%d}", P1.read(), P2.read(), P3.read(), P4.read(), P5.read(), P6.read());
jskl 0:5d27c333afa6 131 } else if (strcmp(command,"get_P1")==0) { // get P1 value
jskl 2:9fe6f1e273b4 132 sprintf(command, "{\"P1\":%d}", P1.read()); //
jskl 0:5d27c333afa6 133 } else if(strcmp(command,"set_P1_o")==0) { // set P1 as output
jskl 0:5d27c333afa6 134 P1.output(); //
jskl 0:5d27c333afa6 135 sprintf(command, "P1 is now set as output"); //
jskl 0:5d27c333afa6 136 } else if (strcmp(command,"set_P1_i")==0) { // set P1 as input
jskl 0:5d27c333afa6 137 P1.input(); //
jskl 0:5d27c333afa6 138 sprintf(command, "P1 is now set as input"); //
jskl 0:5d27c333afa6 139 } else if (strcmp(command,"set_P1_1")==0) { // set P1 as 1
jskl 0:5d27c333afa6 140 P1.output(); // set to output
jskl 0:5d27c333afa6 141 P1.write(1); // write 1 to P1
jskl 2:9fe6f1e273b4 142 sprintf(command, "{\"P1\":%d}", P1.read()); // reply with current value to check
jskl 0:5d27c333afa6 143 } else if (strcmp(command,"set_P1_0")==0) { // set P1 as 0
jskl 0:5d27c333afa6 144 P1.output(); //
jskl 0:5d27c333afa6 145 P1.write(0);
jskl 2:9fe6f1e273b4 146 sprintf(command, "{\"P1\":%d}", P1.read());
jskl 0:5d27c333afa6 147 } else if (strcmp(command,"get_P2")==0) {
jskl 2:9fe6f1e273b4 148 sprintf(command, "{\"P2\":%d}", P2.read());
jskl 0:5d27c333afa6 149 } else if (strcmp(command,"set_P2_o")==0) {
jskl 0:5d27c333afa6 150 P2.output();
jskl 0:5d27c333afa6 151 sprintf(command, "P2 is now set as output");
jskl 0:5d27c333afa6 152 } else if (strcmp(command,"set_P2_i")==0) {
jskl 0:5d27c333afa6 153 P2.input();
jskl 0:5d27c333afa6 154 sprintf (command, "P2 is now set as input");
jskl 0:5d27c333afa6 155 } else if (strcmp(command,"set_P2_1")==0) {
jskl 0:5d27c333afa6 156 P2.output();
jskl 0:5d27c333afa6 157 P2.write(1);
jskl 0:5d27c333afa6 158 sprintf(command, "P2 value is now: %d ", P2.read());
jskl 0:5d27c333afa6 159 } else if (strcmp(command,"set_P2_0")==0) {
jskl 0:5d27c333afa6 160 P2.output();
jskl 0:5d27c333afa6 161 P2.write(0);
jskl 2:9fe6f1e273b4 162 sprintf(command, "{\"P2\":%d}", P2.read());
jskl 0:5d27c333afa6 163 } else if (strcmp(command,"get_P3")==0) {
jskl 2:9fe6f1e273b4 164 sprintf(command, "{\"P3\":%d}", P3.read());
jskl 0:5d27c333afa6 165 } else if (strcmp(command,"set_P3_o")==0) {
jskl 0:5d27c333afa6 166 P3.output();
jskl 0:5d27c333afa6 167 sprintf(command, "P3 is now set as output");
jskl 0:5d27c333afa6 168 } else if (strcmp(command,"set_P3_i")==0) {
jskl 0:5d27c333afa6 169 P3.input();
jskl 0:5d27c333afa6 170 sprintf (command, "P3 is now set as input");
jskl 0:5d27c333afa6 171 } else if (strcmp(command,"set_P3_1")==0) {
jskl 0:5d27c333afa6 172 P3.output();
jskl 0:5d27c333afa6 173 P3.write(1);
jskl 2:9fe6f1e273b4 174 sprintf(command, "{\"P3\":%d}", P3.read());
jskl 0:5d27c333afa6 175 } else if (strcmp(command,"set_P3_0")==0) {
jskl 0:5d27c333afa6 176 P3.output();
jskl 0:5d27c333afa6 177 P3.write(0);
jskl 0:5d27c333afa6 178 sprintf(command, "P3 value is now: %d ", P3.read());
jskl 0:5d27c333afa6 179 } else if (strcmp(command,"get_P4")==0) {
jskl 2:9fe6f1e273b4 180 sprintf(command, "{\"P4\":%d}", P4.read());
jskl 0:5d27c333afa6 181 } else if (strcmp(command,"set_P4_o")==0) {
jskl 0:5d27c333afa6 182 P4.output();
jskl 0:5d27c333afa6 183 sprintf(command, "P4 is now set as output");
jskl 0:5d27c333afa6 184 } else if (strcmp(command,"set_P4_i")==0) {
jskl 0:5d27c333afa6 185 P4.input();
jskl 0:5d27c333afa6 186 sprintf (command, "P4 is now set as input");
jskl 0:5d27c333afa6 187 } else if (strcmp(command,"set_P4_1")==0) {
jskl 0:5d27c333afa6 188 P4.output();
jskl 0:5d27c333afa6 189 P4.write(1);
jskl 2:9fe6f1e273b4 190 sprintf(command, "{\"P4\":%d}", P4.read());
jskl 0:5d27c333afa6 191 } else if (strcmp(command,"set_P4_0")==0) {
jskl 0:5d27c333afa6 192 P4.output();
jskl 0:5d27c333afa6 193 P4.write(0);
jskl 2:9fe6f1e273b4 194 sprintf(command, "{\"P4\":%d}", P4.read());
jskl 0:5d27c333afa6 195 } else if (strcmp(command,"get_P5")==0) {
jskl 2:9fe6f1e273b4 196 sprintf(command, "{\"P5:\"%d}", P5.read());
jskl 0:5d27c333afa6 197 } else if (strcmp(command,"set_P5_o")==0) {
jskl 0:5d27c333afa6 198 P5.output();
jskl 0:5d27c333afa6 199 sprintf(command, "P5 is now set as output");
jskl 0:5d27c333afa6 200 } else if (strcmp(command,"set_P5_i")==0) {
jskl 0:5d27c333afa6 201 P5.input();
jskl 0:5d27c333afa6 202 sprintf (command, "P5 is now set as input");
jskl 0:5d27c333afa6 203 } else if (strcmp(command,"set_P5_1")==0) {
jskl 0:5d27c333afa6 204 P5.output();
jskl 0:5d27c333afa6 205 P5.write(1);
jskl 2:9fe6f1e273b4 206 sprintf(command, "{\"P5\":%d}", P5.read());
jskl 0:5d27c333afa6 207 } else if (strcmp(command,"set_P5_0")==0) {
jskl 0:5d27c333afa6 208 P5.output();
jskl 0:5d27c333afa6 209 P5.write(0);
jskl 2:9fe6f1e273b4 210 sprintf(command, "{\"P5\":%d}", P5.read());
jskl 0:5d27c333afa6 211 } else if (strcmp(command,"get_P6")==0) {
jskl 2:9fe6f1e273b4 212 sprintf(command, "{\"P6\":%d}", P6.read());
jskl 0:5d27c333afa6 213 } else if (strcmp(command,"set_P6_o")==0) {
jskl 0:5d27c333afa6 214 P6.output();
jskl 0:5d27c333afa6 215 sprintf(command, "P6 is now set as output");
jskl 0:5d27c333afa6 216 } else if (strcmp(command,"set_P6_i")==0) {
jskl 0:5d27c333afa6 217 P6.input();
jskl 0:5d27c333afa6 218 sprintf (command, "P6 is now set as input");
jskl 0:5d27c333afa6 219 } else if (strcmp(command,"set_P6_1")==0) {
jskl 0:5d27c333afa6 220 P6.output();
jskl 0:5d27c333afa6 221 P6.write(1);
jskl 2:9fe6f1e273b4 222 sprintf(command, "{\"P6\":%d}", P6.read());
jskl 0:5d27c333afa6 223 } else if (strcmp(command,"set_P6_0")==0) {
jskl 0:5d27c333afa6 224 P6.output();
jskl 0:5d27c333afa6 225 P6.write(0);
jskl 2:9fe6f1e273b4 226 sprintf(command, "{\"P6\":%d}", P6.read());
jskl 0:5d27c333afa6 227 } else sprintf(command, "Unknown Command!");
jskl 0:5d27c333afa6 228 }
jskl 0:5d27c333afa6 229
jskl 0:5d27c333afa6 230 void SerialInterHandler()
jskl 0:5d27c333afa6 231 {
jskl 0:5d27c333afa6 232 /**
jskl 0:5d27c333afa6 233 Enables to configure Ethernet card via serial port in first 5 seconds from KL25Z startup.
jskl 0:5d27c333afa6 234 */
jskl 0:5d27c333afa6 235 pc.getc(); // gets 1 char from buffer and clears it
jskl 0:5d27c333afa6 236 pc.printf("\n \rUse DHCP? \n \ry-yes ");
jskl 0:5d27c333afa6 237 char from_serial;
jskl 0:5d27c333afa6 238 while (true) {
jskl 0:5d27c333afa6 239 if (pc.readable()) {
jskl 0:5d27c333afa6 240 from_serial= pc.getc();
jskl 0:5d27c333afa6 241 break;
jskl 0:5d27c333afa6 242 }
jskl 0:5d27c333afa6 243 }
jskl 0:5d27c333afa6 244 pc.printf("\n \r%c", from_serial);
jskl 0:5d27c333afa6 245 if (from_serial=='y') {
jskl 0:5d27c333afa6 246 USE_DHCP = true;
jskl 0:5d27c333afa6 247 pc.printf("\n \rUsing DHCP ");
jskl 0:5d27c333afa6 248 return;
jskl 0:5d27c333afa6 249 }
jskl 0:5d27c333afa6 250 else if (from_serial=='n') {
jskl 0:5d27c333afa6 251 USE_DHCP = false;
jskl 0:5d27c333afa6 252 pc.printf("\n \rStatic IP configuration ");
jskl 0:5d27c333afa6 253 pc.printf("\n \rCurrent config:");
jskl 0:5d27c333afa6 254 pc.printf("\r \nIP: %s, MASK: %s, GW: %s\n", IP_Addr, IP_Subnet, IP_Gateway);
jskl 0:5d27c333afa6 255 pc.printf("\n \rDo you want to change it?");
jskl 0:5d27c333afa6 256 if(pc.getc()=='y'){
jskl 0:5d27c333afa6 257 pc.printf("\n \rIP: \n \r Press 'enter and then backspace' to end and save \r \n Remember to Enter all 15 characters!\n \r");
jskl 0:5d27c333afa6 258 pc.scanf("%s" ,IP_Addr);
jskl 0:5d27c333afa6 259 pc.printf("new IP: %s", IP_Addr);
jskl 0:5d27c333afa6 260 pc.printf("\n \r Enter Subnet Mask:");
jskl 0:5d27c333afa6 261 pc.scanf("%s", IP_Subnet);
jskl 0:5d27c333afa6 262 pc.printf("\n \r new Mask: %s", IP_Subnet);
jskl 0:5d27c333afa6 263 pc.printf("\n \r Enter Gateway:");
jskl 0:5d27c333afa6 264 pc.scanf("%s", IP_Gateway);
jskl 0:5d27c333afa6 265 pc.printf("\r \n IP: %s, MASK: %s, GW: %s\n", IP_Addr, IP_Subnet, IP_Gateway);
jskl 0:5d27c333afa6 266 }
jskl 0:5d27c333afa6 267 else return;
jskl 0:5d27c333afa6 268 return;
jskl 0:5d27c333afa6 269 }
jskl 0:5d27c333afa6 270 else pc.printf("\n \rWrong key! Nothing to do");
jskl 0:5d27c333afa6 271 }
jskl 0:5d27c333afa6 272
jskl 0:5d27c333afa6 273 void menu_prop()
jskl 0:5d27c333afa6 274 {
jskl 0:5d27c333afa6 275 /**
jskl 0:5d27c333afa6 276 Counting down time left to enter menu.
jskl 0:5d27c333afa6 277 */
jskl 0:5d27c333afa6 278 pc.printf("\n \r Press any key to enter menu \n \r 5");
jskl 0:5d27c333afa6 279 wait(1);
jskl 0:5d27c333afa6 280 pc.printf("\n \r 4");
jskl 0:5d27c333afa6 281 wait(1);
jskl 0:5d27c333afa6 282 pc.printf("\n \r 3");
jskl 0:5d27c333afa6 283 wait(1);
jskl 0:5d27c333afa6 284 pc.printf("\n \r 2");
jskl 0:5d27c333afa6 285 wait(1);
jskl 0:5d27c333afa6 286 pc.printf("\n \r 1");
jskl 0:5d27c333afa6 287 wait(1);
jskl 0:5d27c333afa6 288 pc.printf("\n \r 0 \n \r Starting!");
jskl 0:5d27c333afa6 289
jskl 0:5d27c333afa6 290 }