William Lange / Mbed 2 deprecated DistanceFirmware

Same as mallet... but distance

Dependencies:   EthernetInterface NetworkAPI mbed-rtos mbed

Fork of MalletFirmware by Impact-Echo

main.cpp@25:abbc19af13f9, 2014-12-03 (annotated)

Committer:
timmey9
Date:
Wed Dec 03 09:21:55 2014 +0000
Revision:
25:abbc19af13f9
Parent:
24:a5891669afc5
Child:
26:a00bf9837e03
Everything compiles and works, except when TOTAL_SAMPLES is too large.  Works at 100, and ethernet boots up at 10000 but can't connect to MATLAB.  Check for memory leaks.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
timmey9 20:f533b3c9296f 1 // Server code
donatien 0:bb128f0e952f 2 #include "mbed.h"
timmey9 24:a5891669afc5 3 #include <stdio.h>
timmey9 22:523e316cbe70 4
timmey9 22:523e316cbe70 5 // Ethernet
donatien 0:bb128f0e952f 6 #include "EthernetInterface.h"
timmey9 17:2f978f823020 7 #include "NetworkAPI/buffer.hpp"
timmey9 17:2f978f823020 8 #include "NetworkAPI/select.hpp"
timmey9 17:2f978f823020 9 #include "NetworkAPI/ip/address.hpp"
timmey9 17:2f978f823020 10 #include "NetworkAPI/tcp/socket.hpp"
timmey9 18:b17ddeeb1c09 11
timmey9 22:523e316cbe70 12
timmey9 22:523e316cbe70 13 // Angle encoder and motor control
timmey9 22:523e316cbe70 14 #include "AngleEncoder.h"
timmey9 22:523e316cbe70 15 #include "MotorControl.h"
timmey9 22:523e316cbe70 16
timmey9 22:523e316cbe70 17 // Analog sampling
timmey9 22:523e316cbe70 18 #include "PeripheralNames.h"
timmey9 22:523e316cbe70 19 #include "PeripheralPins.h"
timmey9 22:523e316cbe70 20 #include "fsl_adc_hal.h"
timmey9 22:523e316cbe70 21 #include "fsl_clock_manager.h"
timmey9 22:523e316cbe70 22 #include "fsl_dspi_hal.h"
timmey9 22:523e316cbe70 23 #include "AngleEncoder.h"
timmey9 22:523e316cbe70 24
timmey9 22:523e316cbe70 25
timmey9 22:523e316cbe70 26 /*****************************************
timmey9 22:523e316cbe70 27 *
timmey9 22:523e316cbe70 28 * Configuration
timmey9 22:523e316cbe70 29 *
timmey9 22:523e316cbe70 30 * Take the time to set these constants
timmey9 22:523e316cbe70 31 *
timmey9 22:523e316cbe70 32 *****************************************/
timmey9 21:1fb5023b72af 33 #define MALLET 6 // set mallet to a value between 1-7
timmey9 21:1fb5023b72af 34 #define STATIC 1 // set STATIC to 1 for static ip, set STATIC to 0 for dynamic
timmey9 21:1fb5023b72af 35 #define PORT 22 // set to a random port number. All the mallets can use the same port number.
timmey9 21:1fb5023b72af 36 #define MAX_CLIENTS 2 // set the max number of clients to at least 2 (first client is MATLAB, second is the distance unit)
timmey9 22:523e316cbe70 37 #define INVERT_ANGLE 0 // inverts whether the angle encoder counts up or down
timmey9 22:523e316cbe70 38
timmey9 22:523e316cbe70 39
timmey9 21:1fb5023b72af 40
timmey9 21:1fb5023b72af 41
timmey9 21:1fb5023b72af 42
timmey9 22:523e316cbe70 43 // Analog sampling
timmey9 22:523e316cbe70 44 #define MAX_FADC 6000000
timmey9 22:523e316cbe70 45 #define SAMPLING_RATE 10000 // In microseconds, so 10 us will be a sampling rate of 100 kHz
timmey9 25:abbc19af13f9 46 #define TOTAL_SAMPLES 10000 // originally 30000 for 0.3 ms of sampling.
timmey9 22:523e316cbe70 47
timmey9 22:523e316cbe70 48 #define LAST_SAMPLE_INDEX (TOTAL_SAMPLES-1) // If sampling time is 25 us, then 2000 corresponds to 50 ms
timmey9 22:523e316cbe70 49 #define FIRST_SAMPLE_INDEX 0
timmey9 22:523e316cbe70 50 #define BEGIN_SAMPLING 0xFFFFFFFF
timmey9 22:523e316cbe70 51 #define WAITING_TO_BEGIN (BEGIN_SAMPLING-1)
timmey9 22:523e316cbe70 52
timmey9 22:523e316cbe70 53 #define CHANNEL_STORAGE_OFFSET 16 // For storing the 16 bits and the 16 bits in a single 32 bit array
timmey9 22:523e316cbe70 54 #define PERIOD 3000 // make sure PERIOD >= ON_OFF_TIME
timmey9 22:523e316cbe70 55 #define ON_OFF_TIME 300 // time it takes for relay to turn on
timmey9 22:523e316cbe70 56
timmey9 22:523e316cbe70 57
timmey9 22:523e316cbe70 58 // Ethernet
timmey9 22:523e316cbe70 59 #define GATEWAY "169.254.225.1"
timmey9 22:523e316cbe70 60 #define MASK "255.255.0.0"
timmey9 22:523e316cbe70 61
timmey9 22:523e316cbe70 62 // used for assign different mallets their ip addresses
timmey9 21:1fb5023b72af 63 #if MALLET == 1
timmey9 22:523e316cbe70 64 #define IP "169.254.225.206"
timmey9 21:1fb5023b72af 65 #define NAME "Mallet1\n\r"
timmey9 21:1fb5023b72af 66
timmey9 21:1fb5023b72af 67 #elif MALLET == 2
timmey9 22:523e316cbe70 68 #define IP "169.254.225.207"
timmey9 21:1fb5023b72af 69 #define NAME "Mallet2\n\r"
timmey9 21:1fb5023b72af 70
timmey9 21:1fb5023b72af 71 #elif MALLET == 3
timmey9 22:523e316cbe70 72 #define IP "169.254.225.208"
timmey9 21:1fb5023b72af 73 #define NAME "Mallet3\n\r"
timmey9 21:1fb5023b72af 74
timmey9 21:1fb5023b72af 75 #elif MALLET == 4
timmey9 22:523e316cbe70 76 #define IP "169.254.225.209"
timmey9 21:1fb5023b72af 77 #define NAME "Mallet4\n\r"
timmey9 21:1fb5023b72af 78
timmey9 21:1fb5023b72af 79 #elif MALLET == 5
timmey9 22:523e316cbe70 80 #define IP "169.254.225.210"
timmey9 21:1fb5023b72af 81 #define NAME "Mallet5\n\r"
timmey9 21:1fb5023b72af 82
timmey9 21:1fb5023b72af 83 #elif MALLET == 6
timmey9 22:523e316cbe70 84 #define IP "169.254.225.211"
timmey9 21:1fb5023b72af 85 #define NAME "Mallet6\n\r"
timmey9 21:1fb5023b72af 86
timmey9 21:1fb5023b72af 87 #elif MALLET == 7
timmey9 22:523e316cbe70 88 #define IP "169.254.225.212"
timmey9 21:1fb5023b72af 89 #define NAME "Mallet7\n\r"
timmey9 21:1fb5023b72af 90
timmey9 20:f533b3c9296f 91 #endif
timmey9 20:f533b3c9296f 92
timmey9 21:1fb5023b72af 93
timmey9 22:523e316cbe70 94 // for debug purposes
timmey9 18:b17ddeeb1c09 95 Serial pc(USBTX, USBRX);
timmey9 18:b17ddeeb1c09 96 DigitalOut led_red(LED_RED);
timmey9 18:b17ddeeb1c09 97 DigitalOut led_green(LED_GREEN);
timmey9 18:b17ddeeb1c09 98 DigitalOut led_blue(LED_BLUE);
timmey9 18:b17ddeeb1c09 99
timmey9 23:9e5141647775 100
timmey9 22:523e316cbe70 101 // motor control and angle encoder
timmey9 22:523e316cbe70 102 MotorControl motor(PTC2, PTA2, PERIOD, ON_OFF_TIME); // forward, backward, period, safetyPeriod
timmey9 22:523e316cbe70 103 AngleEncoder angle_encoder(PTD2, PTD3, PTD1, PTD0, 8, 0, 1000000); // mosi, miso, sclk, cs, bit_width, mode, hz
timmey9 22:523e316cbe70 104 DigitalIn AMT20_A(PTC0); // input for quadrature encoding from angle encoder
timmey9 22:523e316cbe70 105 DigitalIn AMT20_B(PTC1); // input for quadrature encoding from angle encoder
timmey9 22:523e316cbe70 106
timmey9 22:523e316cbe70 107 // Analog sampling
timmey9 23:9e5141647775 108 //Ticker Sampler;
timmey9 23:9e5141647775 109 //Timer timer;
timmey9 23:9e5141647775 110 //Timer timeStamp;
timmey9 22:523e316cbe70 111 AnalogIn A0_pin(A0);
timmey9 22:523e316cbe70 112 AnalogIn A2_pin(A2);
timmey9 22:523e316cbe70 113
timmey9 22:523e316cbe70 114 //DigitalIn SW3_switch(PTA4);
timmey9 22:523e316cbe70 115 //DigitalIn SW2_switch(PTC6);
timmey9 25:abbc19af13f9 116 DigitalOut StatusSensor(PTC4);
timmey9 25:abbc19af13f9 117 DigitalOut StatusIndicator2(PTA0); // originally PTD0 but needed for CS for spi
timmey9 22:523e316cbe70 118
timmey9 20:f533b3c9296f 119
timmey9 22:523e316cbe70 120
timmey9 22:523e316cbe70 121 uint32_t current_sample_index = WAITING_TO_BEGIN;
timmey9 25:abbc19af13f9 122 uint16_t sample_array1[TOTAL_SAMPLES];
timmey9 25:abbc19af13f9 123 uint16_t sample_array2[TOTAL_SAMPLES];
timmey9 22:523e316cbe70 124 uint16_t angle_array[TOTAL_SAMPLES];
timmey9 22:523e316cbe70 125
timmey9 22:523e316cbe70 126
timmey9 22:523e316cbe70 127 // Declaration of functions
timmey9 22:523e316cbe70 128 void analog_initialization(PinName pin);
timmey9 22:523e316cbe70 129 void output_data(uint32_t iteration_number);
timmey9 22:523e316cbe70 130 void timed_sampling();
timmey9 22:523e316cbe70 131
timmey9 22:523e316cbe70 132
timmey9 22:523e316cbe70 133 // Important globabl variables necessary for the sampling every interval
timmey9 22:523e316cbe70 134 int rotary_count = 0;
timmey9 22:523e316cbe70 135 uint32_t last_AMT20_AB_read = 0;
timmey9 23:9e5141647775 136
timmey9 22:523e316cbe70 137 //using namespace network;
timmey9 22:523e316cbe70 138 using namespace std;
timmey9 17:2f978f823020 139
emilmont 7:65188f4a8c25 140 int main() {
timmey9 23:9e5141647775 141
timmey9 25:abbc19af13f9 142 //for(int i = 0; i < TOTAL_SAMPLES; i++) {sample_array[i] = i;}
timmey9 22:523e316cbe70 143 led_blue = 1;
timmey9 22:523e316cbe70 144 led_green = 1;
timmey9 18:b17ddeeb1c09 145 led_red = 1;
timmey9 22:523e316cbe70 146
timmey9 18:b17ddeeb1c09 147 pc.baud(230400);
timmey9 24:a5891669afc5 148 pc.printf("Starting %s\r\n",NAME);
timmey9 23:9e5141647775 149
timmey9 22:523e316cbe70 150 analog_initialization(A0);
timmey9 22:523e316cbe70 151 analog_initialization(A2);
timmey9 20:f533b3c9296f 152
timmey9 22:523e316cbe70 153 // setup this timer
timmey9 23:9e5141647775 154 //timeStamp.stop();
timmey9 23:9e5141647775 155 //timeStamp.reset();
timmey9 22:523e316cbe70 156
timmey9 22:523e316cbe70 157 // Start the sampling loop
timmey9 22:523e316cbe70 158 current_sample_index = WAITING_TO_BEGIN;
timmey9 23:9e5141647775 159 //Sampler.attach_us(&timed_sampling, SAMPLING_RATE);
timmey9 23:9e5141647775 160 //__disable_irq(); // Disable Interrupts
timmey9 23:9e5141647775 161 //timer.reset();
timmey9 23:9e5141647775 162 //timer.start();
timmey9 22:523e316cbe70 163
timmey9 22:523e316cbe70 164 uint32_t trigger_count = 0;
timmey9 22:523e316cbe70 165
timmey9 22:523e316cbe70 166 // corresponding duty 1 0 0.7 1 0.75
timmey9 22:523e316cbe70 167 uint32_t duration[8] = {10000, 60000, 10000, 39000, 7000, 7000, 0, 0};
timmey9 22:523e316cbe70 168 //uint32_t duration[8] = {10000, 100000, 10000, 92000, 25000, 7000, 0, 0};
timmey9 25:abbc19af13f9 169 //int32_t pointer = 0;
timmey9 22:523e316cbe70 170
timmey9 22:523e316cbe70 171 double duty_cycle = 0.25;
timmey9 23:9e5141647775 172
timmey9 22:523e316cbe70 173 // flash red LED
timmey9 22:523e316cbe70 174 led_red = 0;
timmey9 22:523e316cbe70 175 wait_ms(500);
timmey9 22:523e316cbe70 176 led_red = 1;
timmey9 22:523e316cbe70 177
timmey9 22:523e316cbe70 178
timmey9 22:523e316cbe70 179
timmey9 22:523e316cbe70 180
timmey9 22:523e316cbe70 181
timmey9 22:523e316cbe70 182
timmey9 22:523e316cbe70 183
timmey9 22:523e316cbe70 184 // initialize ethernet connection and server
timmey9 17:2f978f823020 185 EthernetInterface interface;
timmey9 22:523e316cbe70 186
timmey9 21:1fb5023b72af 187 #if STATIC == 1
timmey9 20:f533b3c9296f 188 interface.init(IP, MASK, GATEWAY);
timmey9 20:f533b3c9296f 189 #else
timmey9 17:2f978f823020 190 interface.init();
timmey9 20:f533b3c9296f 191 #endif
timmey9 21:1fb5023b72af 192
timmey9 17:2f978f823020 193 interface.connect();
timmey9 24:a5891669afc5 194 //pc.printf(NAME);
timmey9 20:f533b3c9296f 195 pc.printf("IP Address is: %s\n\r", interface.getIPAddress());
timmey9 20:f533b3c9296f 196 pc.printf("Network Mask is: %s\n\r", interface.getNetworkMask());
timmey9 20:f533b3c9296f 197 pc.printf("MAC address is: %s\n\r", interface.getMACAddress());
timmey9 20:f533b3c9296f 198 pc.printf("Gateway is: %s\n\r", interface.getGateway());
timmey9 21:1fb5023b72af 199 pc.printf("Port is: %i\n\r", PORT);
timmey9 20:f533b3c9296f 200
timmey9 22:523e316cbe70 201 // ethernet setup failed for some reason. Flash yellow light then uC resets itself
timmey9 23:9e5141647775 202 if(interface.getIPAddress() == 0)
timmey9 22:523e316cbe70 203 {
timmey9 22:523e316cbe70 204 for(int i = 0; i < 5; i++)
timmey9 22:523e316cbe70 205 {
timmey9 22:523e316cbe70 206 led_red = 0;
timmey9 22:523e316cbe70 207 led_green = 0;
timmey9 22:523e316cbe70 208 wait_ms(500);
timmey9 22:523e316cbe70 209 led_red = 1;
timmey9 22:523e316cbe70 210 led_green = 1;
timmey9 22:523e316cbe70 211 wait_ms(1000);
timmey9 22:523e316cbe70 212 }
timmey9 22:523e316cbe70 213 NVIC_SystemReset();
timmey9 23:9e5141647775 214 }
timmey9 22:523e316cbe70 215
timmey9 22:523e316cbe70 216 network::Select select;
timmey9 22:523e316cbe70 217 network::tcp::Socket server;
timmey9 22:523e316cbe70 218 network::tcp::Socket client[MAX_CLIENTS];
timmey9 22:523e316cbe70 219 network::tcp::Socket *socket = NULL;
timmey9 17:2f978f823020 220
timmey9 17:2f978f823020 221 int result = 0;
timmey9 17:2f978f823020 222 int index = 0;
timmey9 17:2f978f823020 223
timmey9 22:523e316cbe70 224 network::Buffer buffer(TOTAL_SAMPLES);
timmey9 21:1fb5023b72af 225 std::string message(NAME);
timmey9 17:2f978f823020 226
timmey9 21:1fb5023b72af 227 // Configure the server socket (assume every thing works)
timmey9 17:2f978f823020 228 server.open();
timmey9 20:f533b3c9296f 229 server.bind(PORT);
timmey9 17:2f978f823020 230 server.listen(MAX_CLIENTS);
timmey9 17:2f978f823020 231
timmey9 17:2f978f823020 232 // Add sockets to the select api
timmey9 22:523e316cbe70 233 select.set(&server, network::Select::Read);
timmey9 17:2f978f823020 234 for (index = 0; index < MAX_CLIENTS; index++) {
timmey9 22:523e316cbe70 235 select.set(&client[index], network::Select::Read);
timmey9 17:2f978f823020 236 }
timmey9 22:523e316cbe70 237
timmey9 22:523e316cbe70 238 led_red = 1;
timmey9 22:523e316cbe70 239 led_green = 1;
timmey9 22:523e316cbe70 240 led_blue = 0;
timmey9 23:9e5141647775 241 wait_ms(500);
timmey9 22:523e316cbe70 242 led_blue = 1;
timmey9 23:9e5141647775 243 wait_ms(200);
timmey9 23:9e5141647775 244 led_blue = 0;
timmey9 23:9e5141647775 245 wait_ms(500);
timmey9 23:9e5141647775 246 led_blue = 1;
timmey9 22:523e316cbe70 247
timmey9 17:2f978f823020 248 do {
timmey9 17:2f978f823020 249 // Wait for activity
timmey9 17:2f978f823020 250 result = select.wait();
timmey9 17:2f978f823020 251 if (result < -1) {
timmey9 18:b17ddeeb1c09 252 pc.printf("Failed to select\n\r");
emilmont 7:65188f4a8c25 253 break;
timmey9 16:c3f922f61b8f 254 }
timmey9 17:2f978f823020 255
timmey9 17:2f978f823020 256 // Get the first socket
timmey9 22:523e316cbe70 257 socket = (network::tcp::Socket *)select.getReadable();
timmey9 17:2f978f823020 258
timmey9 22:523e316cbe70 259 for (; socket != NULL; socket = (network::tcp::Socket *)select.getReadable()) {
timmey9 17:2f978f823020 260 // Check if there was a connection request.
timmey9 17:2f978f823020 261 if (socket->getHandle() == server.getHandle()) {
timmey9 17:2f978f823020 262 // Find an unused client
timmey9 17:2f978f823020 263 for (index = 0; index < MAX_CLIENTS; index++) {
timmey9 17:2f978f823020 264 if (client[index].getStatus() == network::Socket::Closed) {
timmey9 17:2f978f823020 265 break;
timmey9 17:2f978f823020 266 }
timmey9 17:2f978f823020 267 }
timmey9 17:2f978f823020 268
timmey9 17:2f978f823020 269 // Maximum connections reached
timmey9 17:2f978f823020 270 if (index == MAX_CLIENTS) {
timmey9 18:b17ddeeb1c09 271 pc.printf("Maximum connections reached\n\r");
timmey9 21:1fb5023b72af 272 wait(1);
timmey9 17:2f978f823020 273 continue;
timmey9 17:2f978f823020 274 }
timmey9 20:f533b3c9296f 275
timmey9 17:2f978f823020 276 // Accept the client
timmey9 17:2f978f823020 277 socket->accept(client[index]);
timmey9 18:b17ddeeb1c09 278 pc.printf("Client connected %s:%d\n\r",
timmey9 17:2f978f823020 279 client[index].getRemoteEndpoint().getAddress().toString().c_str(),
timmey9 17:2f978f823020 280 client[index].getRemoteEndpoint().getPort());
timmey9 17:2f978f823020 281
timmey9 21:1fb5023b72af 282 // Send a nice message to the client (tell MATLAB your name
timmey9 17:2f978f823020 283 client[index].write((void *)message.data(), message.size());
timmey9 20:f533b3c9296f 284
timmey9 20:f533b3c9296f 285 // read some registers for some info.
timmey9 20:f533b3c9296f 286 //uint32_t* rcr = (uint32_t*) 0x400C0084;
timmey9 20:f533b3c9296f 287 //uint32_t* ecr = (uint32_t*) 0x400C0024;
timmey9 20:f533b3c9296f 288 //pc.printf("RCR register: %x\r\n", *rcr);
timmey9 20:f533b3c9296f 289 //pc.printf("ECR register: %x\r\n", *ecr);
timmey9 20:f533b3c9296f 290
timmey9 17:2f978f823020 291 continue;
timmey9 16:c3f922f61b8f 292 }
timmey9 20:f533b3c9296f 293
timmey9 17:2f978f823020 294 // It was not the server socket, so it must be a client talking to us.
timmey9 17:2f978f823020 295 switch (socket->read(buffer)) {
timmey9 17:2f978f823020 296 case 0:
timmey9 17:2f978f823020 297 // Remote end disconnected
timmey9 18:b17ddeeb1c09 298 pc.printf("Client disconnected %s:%d\n\r",
timmey9 17:2f978f823020 299 socket->getRemoteEndpoint().getAddress().toString().c_str(),
timmey9 17:2f978f823020 300 socket->getRemoteEndpoint().getPort());
timmey9 17:2f978f823020 301
timmey9 17:2f978f823020 302 // Close socket
timmey9 17:2f978f823020 303 socket->close();
timmey9 17:2f978f823020 304 break;
timmey9 20:f533b3c9296f 305
timmey9 17:2f978f823020 306 case -1:
timmey9 18:b17ddeeb1c09 307 pc.printf("Error while reading data from socket\n\r");
timmey9 17:2f978f823020 308 socket->close();
timmey9 17:2f978f823020 309 break;
timmey9 20:f533b3c9296f 310 //************* this is where data is printed to the screen
timmey9 17:2f978f823020 311 default:
timmey9 18:b17ddeeb1c09 312 pc.printf("Message from %s:%d\n\r",
timmey9 17:2f978f823020 313 socket->getRemoteEndpoint().getAddress().toString().c_str(),
timmey9 17:2f978f823020 314 socket->getRemoteEndpoint().getPort());
timmey9 17:2f978f823020 315
timmey9 21:1fb5023b72af 316 pc.printf("%s\n\r", (char *)buffer.data());
timmey9 21:1fb5023b72af 317
timmey9 21:1fb5023b72af 318 // read first character for command
timmey9 21:1fb5023b72af 319 char command[2];
timmey9 21:1fb5023b72af 320 buffer.read(command,2,0);
timmey9 21:1fb5023b72af 321 if(command[1] == ':') {
timmey9 21:1fb5023b72af 322 switch(command[0])
timmey9 21:1fb5023b72af 323 {
timmey9 21:1fb5023b72af 324 case 'b':
timmey9 21:1fb5023b72af 325 led_blue = !led_blue;
timmey9 21:1fb5023b72af 326 client[index].write((void *)"Blue LED\n",9);
timmey9 21:1fb5023b72af 327 break;
timmey9 25:abbc19af13f9 328
timmey9 21:1fb5023b72af 329 case 'r':
timmey9 21:1fb5023b72af 330 led_red = !led_red;
timmey9 21:1fb5023b72af 331 client[index].write((void *)"Red LED\n",8);
timmey9 21:1fb5023b72af 332 break;
timmey9 25:abbc19af13f9 333
timmey9 21:1fb5023b72af 334 case 'p':
timmey9 21:1fb5023b72af 335 led_green = !led_green;
timmey9 21:1fb5023b72af 336 client[index].write((void *)"Data\n",5);
timmey9 25:abbc19af13f9 337 client[index].write((void *)&sample_array1,2*TOTAL_SAMPLES);
timmey9 24:a5891669afc5 338 break;
timmey9 25:abbc19af13f9 339
timmey9 25:abbc19af13f9 340 case '1': // run motor and sample
timmey9 25:abbc19af13f9 341 {
timmey9 25:abbc19af13f9 342 pc.printf("All duration settings:\r\n");
timmey9 25:abbc19af13f9 343 for(int i = 0; i < 8; i++)
timmey9 25:abbc19af13f9 344 {
timmey9 25:abbc19af13f9 345 pc.printf("Duration[%i]: %i\r\n", i, duration[i]);
timmey9 25:abbc19af13f9 346 }
timmey9 25:abbc19af13f9 347 // release mallet
timmey9 25:abbc19af13f9 348 // add code here
timmey9 25:abbc19af13f9 349
timmey9 25:abbc19af13f9 350 //__enable_irq(); // Enable Interrupts
timmey9 25:abbc19af13f9 351
timmey9 25:abbc19af13f9 352 /*current_sample_index = BEGIN_SAMPLING;
timmey9 25:abbc19af13f9 353 trigger_count++;
timmey9 25:abbc19af13f9 354 while (current_sample_index != WAITING_TO_BEGIN){
timmey9 25:abbc19af13f9 355 wait_us(10);
timmey9 25:abbc19af13f9 356 }*/
timmey9 25:abbc19af13f9 357
timmey9 25:abbc19af13f9 358 // below is a crappy form of sampling *******************************
timmey9 25:abbc19af13f9 359 rotary_count = 0;
timmey9 25:abbc19af13f9 360 for(int i = 0; i < TOTAL_SAMPLES; i++)
timmey9 25:abbc19af13f9 361 {
timmey9 25:abbc19af13f9 362 StatusIndicator2 = !StatusIndicator2;
timmey9 25:abbc19af13f9 363
timmey9 25:abbc19af13f9 364 uint32_t A0_value = adc_hal_get_conversion_value(0, 0);
timmey9 25:abbc19af13f9 365 uint32_t A2_value = adc_hal_get_conversion_value(1, 0);
timmey9 25:abbc19af13f9 366 BW_ADC_SC1n_ADCH(0, 0, kAdcChannel12); // This corresponds to starting an ADC conversion on channel 12 of ADC 0 - which is A0 (PTB2)
timmey9 25:abbc19af13f9 367 BW_ADC_SC1n_ADCH(1, 0, kAdcChannel14); // This corresponds to starting an ADC conversion on channel 14 of ADC 1 - which is A2 (PTB10)
timmey9 25:abbc19af13f9 368
timmey9 25:abbc19af13f9 369 // The following updates the rotary counter for the AMT20 sensor
timmey9 25:abbc19af13f9 370 // Put A on PTC0
timmey9 25:abbc19af13f9 371 // Put B on PTC1
timmey9 25:abbc19af13f9 372 uint32_t AMT20_AB = HW_GPIO_PDIR_RD(HW_PORTC) & 0x03;
timmey9 25:abbc19af13f9 373 //AMT20_AB = ~last_AMT20_AB_read; // Used to force a count - extend time.
timmey9 25:abbc19af13f9 374 if (AMT20_AB != last_AMT20_AB_read)
timmey9 25:abbc19af13f9 375 {
timmey9 25:abbc19af13f9 376 // change "INVERT_ANGLE" to change whether relative angle counts up or down.
timmey9 25:abbc19af13f9 377 if ((AMT20_AB >> 1)^(last_AMT20_AB_read) & 1U)
timmey9 25:abbc19af13f9 378 #if INVERT_ANGLE == 1
timmey9 25:abbc19af13f9 379 {rotary_count--;}
timmey9 25:abbc19af13f9 380 else
timmey9 25:abbc19af13f9 381 {rotary_count++;}
timmey9 25:abbc19af13f9 382 #else
timmey9 25:abbc19af13f9 383 {rotary_count++;}
timmey9 25:abbc19af13f9 384 else
timmey9 25:abbc19af13f9 385 {rotary_count--;}
timmey9 25:abbc19af13f9 386 #endif
timmey9 25:abbc19af13f9 387
timmey9 25:abbc19af13f9 388 last_AMT20_AB_read = AMT20_AB;
timmey9 25:abbc19af13f9 389 }
timmey9 25:abbc19af13f9 390
timmey9 25:abbc19af13f9 391 sample_array1[i] = A0_value;
timmey9 25:abbc19af13f9 392 sample_array2[i] = A2_value;
timmey9 25:abbc19af13f9 393 angle_array[i] = rotary_count;
timmey9 25:abbc19af13f9 394
timmey9 25:abbc19af13f9 395 wait_us(10);
timmey9 25:abbc19af13f9 396 }
timmey9 25:abbc19af13f9 397 client[index].write((void *)"Data\n",5);
timmey9 25:abbc19af13f9 398 client[index].write((void *)&sample_array1,2*TOTAL_SAMPLES);
timmey9 25:abbc19af13f9 399 client[index].write((void *)&sample_array2,2*TOTAL_SAMPLES);
timmey9 25:abbc19af13f9 400 client[index].write((void *)&angle_array,2*TOTAL_SAMPLES);
timmey9 25:abbc19af13f9 401
timmey9 25:abbc19af13f9 402 //above is a crappy form of sampling ********************************
timmey9 25:abbc19af13f9 403
timmey9 25:abbc19af13f9 404 //__disable_irq(); // Disable Interrupts
timmey9 25:abbc19af13f9 405
timmey9 25:abbc19af13f9 406 // reset mallet
timmey9 25:abbc19af13f9 407 // add code here
timmey9 25:abbc19af13f9 408
timmey9 25:abbc19af13f9 409 output_data(trigger_count);
timmey9 25:abbc19af13f9 410 }
timmey9 24:a5891669afc5 411 break;
timmey9 25:abbc19af13f9 412
timmey9 25:abbc19af13f9 413 case '2': // run just the motor
timmey9 25:abbc19af13f9 414 {
timmey9 25:abbc19af13f9 415 pc.printf("All duration settings 2:\r\n");
timmey9 25:abbc19af13f9 416 for(int i = 0; i < 8; i++)
timmey9 25:abbc19af13f9 417 {
timmey9 25:abbc19af13f9 418 pc.printf("Duration[%i]: %i\r\n", i, duration[i]);
timmey9 25:abbc19af13f9 419 }
timmey9 25:abbc19af13f9 420
timmey9 25:abbc19af13f9 421 // release mallet
timmey9 25:abbc19af13f9 422 motor.forward(duration[0]); // move motor forward
timmey9 25:abbc19af13f9 423 wait_us(duration[1]); // wait
timmey9 25:abbc19af13f9 424 motor.backward(0.7, duration[2]); // stop motor using reverse
timmey9 25:abbc19af13f9 425
timmey9 25:abbc19af13f9 426 // time for sampling
timmey9 25:abbc19af13f9 427 wait_us(SAMPLING_RATE*TOTAL_SAMPLES);
timmey9 25:abbc19af13f9 428
timmey9 25:abbc19af13f9 429 // reset mallet
timmey9 25:abbc19af13f9 430 motor.backward(duration[3]); // move motor backward
timmey9 25:abbc19af13f9 431 motor.backward(0.75, duration[4]);
timmey9 25:abbc19af13f9 432 motor.backward(duty_cycle, duration[5]);
timmey9 25:abbc19af13f9 433 }
timmey9 24:a5891669afc5 434 break;
timmey9 24:a5891669afc5 435 case 'a':
timmey9 24:a5891669afc5 436 if(angle_encoder.set_zero(&rotary_count)) {
timmey9 24:a5891669afc5 437 client[index].write((void *) "Zero set\n",9);
timmey9 24:a5891669afc5 438 }
timmey9 24:a5891669afc5 439 else {
timmey9 24:a5891669afc5 440 client[index].write((void *) "Zero NOT set\n",13);
timmey9 24:a5891669afc5 441 }
timmey9 24:a5891669afc5 442 break;
timmey9 24:a5891669afc5 443 case 's':
timmey9 24:a5891669afc5 444 {
timmey9 24:a5891669afc5 445 char buf[16];
timmey9 24:a5891669afc5 446 sprintf(buf,"NOP: %x\n",angle_encoder.nop());
timmey9 24:a5891669afc5 447 client[index].write((void *) buf,16);
timmey9 24:a5891669afc5 448 break;
timmey9 24:a5891669afc5 449 }
timmey9 24:a5891669afc5 450 case 'd':
timmey9 24:a5891669afc5 451 {
timmey9 24:a5891669afc5 452 char buf[29];
timmey9 24:a5891669afc5 453 sprintf(buf,"Angle: %i %i\n",angle_encoder.absolute_angle(), rotary_count);
timmey9 24:a5891669afc5 454 client[index].write((void *) buf,29);
timmey9 24:a5891669afc5 455 break;
timmey9 24:a5891669afc5 456 }
timmey9 24:a5891669afc5 457 /*
timmey9 24:a5891669afc5 458
timmey9 24:a5891669afc5 459 // for motor testing
timmey9 24:a5891669afc5 460 if(temp == 'k') // motor backward
timmey9 24:a5891669afc5 461 {
timmey9 24:a5891669afc5 462 motor.backward(duty_cycle, duration[5]);
timmey9 24:a5891669afc5 463 }
timmey9 24:a5891669afc5 464
timmey9 24:a5891669afc5 465 if(temp == 'u' && duty_cycle < 1.00f) pc.printf("%f \r\n", duty_cycle += 0.01f);
timmey9 24:a5891669afc5 466 if(temp == 'i' && duty_cycle > 0.00f) pc.printf("%f \r\n", duty_cycle -= 0.01f);
timmey9 24:a5891669afc5 467
timmey9 24:a5891669afc5 468 if(temp == '=') // you can hit the '+' key to increment "duration" without holding down "shift"
timmey9 24:a5891669afc5 469 {
timmey9 24:a5891669afc5 470 if(pointer < 7) pointer++;
timmey9 24:a5891669afc5 471 pc.printf("Duration[%i]: %i\r\n",pointer, duration[pointer]);
timmey9 24:a5891669afc5 472 }
timmey9 24:a5891669afc5 473 if(temp == '-')
timmey9 24:a5891669afc5 474 {
timmey9 24:a5891669afc5 475 if(pointer > 0) pointer--;
timmey9 24:a5891669afc5 476 pc.printf("Duration[%i]: %i\r\n",pointer, duration[pointer]);
timmey9 24:a5891669afc5 477 }
timmey9 24:a5891669afc5 478 if(temp == ']') // you can hit the '+' key to increment "duration" without holding down "shift"
timmey9 24:a5891669afc5 479 {
timmey9 24:a5891669afc5 480 duration[pointer] += 1000;
timmey9 24:a5891669afc5 481 pc.printf(" %i\r\n", duration[pointer]);
timmey9 24:a5891669afc5 482 }
timmey9 24:a5891669afc5 483 if(temp == '[')
timmey9 24:a5891669afc5 484 {
timmey9 24:a5891669afc5 485 if(duration[pointer] > 0) duration[pointer]-= 1000;
timmey9 24:a5891669afc5 486 pc.printf(" %i\r\n", duration[pointer]);
timmey9 24:a5891669afc5 487 }
timmey9 24:a5891669afc5 488
timmey9 24:a5891669afc5 489 }
timmey9 24:a5891669afc5 490
timmey9 24:a5891669afc5 491 */
timmey9 24:a5891669afc5 492
timmey9 24:a5891669afc5 493
timmey9 24:a5891669afc5 494
timmey9 21:1fb5023b72af 495 }
timmey9 21:1fb5023b72af 496 }
timmey9 21:1fb5023b72af 497
timmey9 21:1fb5023b72af 498
timmey9 20:f533b3c9296f 499
timmey9 20:f533b3c9296f 500 //***************** print a message back to the client
timmey9 21:1fb5023b72af 501
timmey9 21:1fb5023b72af 502 //client[index].write((void *)&sample_array,SAMPLES);
timmey9 21:1fb5023b72af 503
timmey9 21:1fb5023b72af 504
timmey9 21:1fb5023b72af 505
timmey9 20:f533b3c9296f 506
timmey9 21:1fb5023b72af 507 /*for(int i = 1; i <= SAMPLES+1;)
timmey9 21:1fb5023b72af 508 {
timmey9 21:1fb5023b72af 509 for(int j = 0; j < 20; j++)
timmey9 21:1fb5023b72af 510 {
timmey9 21:1fb5023b72af 511 Timer timeStamp;
timmey9 21:1fb5023b72af 512 timeStamp.stop();
timmey9 21:1fb5023b72af 513 timeStamp.reset();
timmey9 21:1fb5023b72af 514 timeStamp.start();
timmey9 21:1fb5023b72af 515
timmey9 21:1fb5023b72af 516 client[index].write((void *)&sample_array,i);
timmey9 21:1fb5023b72af 517 int timeStampVar = timeStamp.read_us();
timmey9 21:1fb5023b72af 518 timeStamp.stop();
timmey9 21:1fb5023b72af 519
timmey9 21:1fb5023b72af 520 pc.printf("*******\r\n%i\r\nTime taken to send data: %i\r\n", i,timeStampVar);
timmey9 21:1fb5023b72af 521
timmey9 21:1fb5023b72af 522 char premessage[40];
timmey9 21:1fb5023b72af 523 sprintf(premessage, "******\r\n%i\r\nTime taken to send data: %i\r\n", i, timeStampVar);
timmey9 21:1fb5023b72af 524 std::string response1 = premessage;
timmey9 21:1fb5023b72af 525 client[index].write((void *)response1.data(), response1.size());
timmey9 21:1fb5023b72af 526 wait_us(5000);
timmey9 21:1fb5023b72af 527 }
timmey9 21:1fb5023b72af 528 if(i == 10000) i = SAMPLES;
timmey9 21:1fb5023b72af 529 else if(i == SAMPLES) i = i*10;
timmey9 21:1fb5023b72af 530 else i = i*10;
timmey9 21:1fb5023b72af 531 }
timmey9 21:1fb5023b72af 532 std::string endMessage("end");
timmey9 21:1fb5023b72af 533 client[index].write((void *)endMessage.data(), endMessage.size());
timmey9 21:1fb5023b72af 534 */
timmey9 17:2f978f823020 535 break;
timmey9 17:2f978f823020 536 }
timmey9 16:c3f922f61b8f 537 }
timmey9 17:2f978f823020 538
timmey9 17:2f978f823020 539 } while (server.getStatus() == network::Socket::Listening);
timmey9 22:523e316cbe70 540 }
timmey9 23:9e5141647775 541
timmey9 22:523e316cbe70 542 void timed_sampling() {
timmey9 22:523e316cbe70 543 __disable_irq(); // Disable Interrupts
timmey9 23:9e5141647775 544 //timeStamp.start();
timmey9 22:523e316cbe70 545
timmey9 22:523e316cbe70 546 // The following performs analog-to-digital conversions - first reading the last conversion - then initiating another
timmey9 22:523e316cbe70 547 uint32_t A0_value = adc_hal_get_conversion_value(0, 0);
timmey9 22:523e316cbe70 548 uint32_t A2_value = adc_hal_get_conversion_value(1, 0);
timmey9 22:523e316cbe70 549 BW_ADC_SC1n_ADCH(0, 0, kAdcChannel12); // This corresponds to starting an ADC conversion on channel 12 of ADC 0 - which is A0 (PTB2)
timmey9 22:523e316cbe70 550 BW_ADC_SC1n_ADCH(1, 0, kAdcChannel14); // This corresponds to starting an ADC conversion on channel 14 of ADC 1 - which is A2 (PTB10)
timmey9 22:523e316cbe70 551
timmey9 22:523e316cbe70 552 // The following updates the rotary counter for the AMT20 sensor
timmey9 22:523e316cbe70 553 // Put A on PTC0
timmey9 22:523e316cbe70 554 // Put B on PTC1
timmey9 22:523e316cbe70 555 uint32_t AMT20_AB = HW_GPIO_PDIR_RD(HW_PORTC) & 0x03;
timmey9 22:523e316cbe70 556 //AMT20_AB = ~last_AMT20_AB_read; // Used to force a count - extend time.
timmey9 22:523e316cbe70 557 if (AMT20_AB != last_AMT20_AB_read)
timmey9 22:523e316cbe70 558 {
timmey9 22:523e316cbe70 559 // change "INVERT_ANGLE" to change whether relative angle counts up or down.
timmey9 22:523e316cbe70 560 if ((AMT20_AB >> 1)^(last_AMT20_AB_read) & 1U)
timmey9 22:523e316cbe70 561 #if INVERT_ANGLE == 1
timmey9 22:523e316cbe70 562 {rotary_count--;}
timmey9 22:523e316cbe70 563 else
timmey9 22:523e316cbe70 564 {rotary_count++;}
timmey9 22:523e316cbe70 565 #else
timmey9 22:523e316cbe70 566 {rotary_count++;}
timmey9 22:523e316cbe70 567 else
timmey9 22:523e316cbe70 568 {rotary_count--;}
timmey9 22:523e316cbe70 569 #endif
timmey9 22:523e316cbe70 570
timmey9 22:523e316cbe70 571 last_AMT20_AB_read = AMT20_AB;
timmey9 22:523e316cbe70 572 }
timmey9 22:523e316cbe70 573 //current_sample_index = BEGIN_SAMPLING; // Used to force extra time.
timmey9 22:523e316cbe70 574 if (current_sample_index == WAITING_TO_BEGIN) {}
timmey9 22:523e316cbe70 575 else
timmey9 22:523e316cbe70 576 {
timmey9 22:523e316cbe70 577 if (current_sample_index == BEGIN_SAMPLING) {
timmey9 22:523e316cbe70 578 current_sample_index = FIRST_SAMPLE_INDEX;
timmey9 22:523e316cbe70 579 }
timmey9 22:523e316cbe70 580
timmey9 25:abbc19af13f9 581 sample_array1[current_sample_index] = (A0_value << CHANNEL_STORAGE_OFFSET) | A2_value;
timmey9 22:523e316cbe70 582
timmey9 22:523e316cbe70 583 angle_array[current_sample_index] = rotary_count;
timmey9 22:523e316cbe70 584
timmey9 22:523e316cbe70 585 if (current_sample_index == LAST_SAMPLE_INDEX) {
timmey9 22:523e316cbe70 586 current_sample_index = WAITING_TO_BEGIN;
timmey9 22:523e316cbe70 587 //led_green = 1;
timmey9 22:523e316cbe70 588 }
timmey9 22:523e316cbe70 589 else { current_sample_index++; }
timmey9 22:523e316cbe70 590 }
timmey9 22:523e316cbe70 591
timmey9 23:9e5141647775 592 //int tempVar = timeStamp.read_us();
timmey9 23:9e5141647775 593 //timeStamp.stop();
timmey9 23:9e5141647775 594 //timeStamp.reset();
timmey9 23:9e5141647775 595 //pc.printf("TimeStamp: %i\r\n", tempVar);
timmey9 22:523e316cbe70 596 __enable_irq(); // Enable Interrupts
timmey9 22:523e316cbe70 597 }
timmey9 22:523e316cbe70 598
timmey9 22:523e316cbe70 599 void analog_initialization(PinName pin)
timmey9 22:523e316cbe70 600 {
timmey9 22:523e316cbe70 601 ADCName adc = (ADCName)pinmap_peripheral(pin, PinMap_ADC);
timmey9 22:523e316cbe70 602 // MBED_ASSERT(adc != (ADCName)NC);
timmey9 22:523e316cbe70 603
timmey9 22:523e316cbe70 604 uint32_t instance = adc >> ADC_INSTANCE_SHIFT;
timmey9 22:523e316cbe70 605
timmey9 22:523e316cbe70 606 clock_manager_set_gate(kClockModuleADC, instance, true);
timmey9 22:523e316cbe70 607
timmey9 22:523e316cbe70 608 uint32_t bus_clock;
timmey9 22:523e316cbe70 609 clock_manager_get_frequency(kBusClock, &bus_clock);
timmey9 22:523e316cbe70 610 uint32_t clkdiv;
timmey9 22:523e316cbe70 611 for (clkdiv = 0; clkdiv < 4; clkdiv++) {
timmey9 22:523e316cbe70 612 if ((bus_clock >> clkdiv) <= MAX_FADC)
timmey9 22:523e316cbe70 613 break;
timmey9 22:523e316cbe70 614 }
timmey9 22:523e316cbe70 615 if (clkdiv == 4) {
timmey9 22:523e316cbe70 616 clkdiv = 0x7; //Set max div
timmey9 22:523e316cbe70 617 }
timmey9 22:523e316cbe70 618 // adc is enabled/triggered when reading.
timmey9 22:523e316cbe70 619 adc_hal_set_clock_source_mode(instance, (adc_clock_source_mode_t)(clkdiv >> 2));
timmey9 22:523e316cbe70 620 adc_hal_set_clock_divider_mode(instance, (adc_clock_divider_mode_t)(clkdiv & 0x3));
timmey9 22:523e316cbe70 621 adc_hal_set_reference_voltage_mode(instance, kAdcVoltageVref);
timmey9 22:523e316cbe70 622 adc_hal_set_resolution_mode(instance, kAdcSingleDiff16);
timmey9 22:523e316cbe70 623 adc_hal_configure_continuous_conversion(instance, false);
timmey9 22:523e316cbe70 624 adc_hal_configure_hw_trigger(instance, false); // sw trigger
timmey9 22:523e316cbe70 625 adc_hal_configure_hw_average(instance, false);
timmey9 22:523e316cbe70 626 adc_hal_set_hw_average_mode(instance, kAdcHwAverageCount4);
timmey9 22:523e316cbe70 627 adc_hal_set_group_mux(instance, kAdcChannelMuxB); // only B channels are avail
timmey9 22:523e316cbe70 628
timmey9 22:523e316cbe70 629 pinmap_pinout(pin, PinMap_ADC);
timmey9 22:523e316cbe70 630 }
timmey9 22:523e316cbe70 631
timmey9 22:523e316cbe70 632 void output_data(uint32_t iteration_number)
timmey9 22:523e316cbe70 633 {
timmey9 22:523e316cbe70 634 pc.printf("Iteration: %i\n\r", iteration_number);
timmey9 22:523e316cbe70 635 pc.printf("Sampling rate: %i\n\r", SAMPLING_RATE);
timmey9 22:523e316cbe70 636 pc.printf("Data length: %i\n\r", TOTAL_SAMPLES);
timmey9 22:523e316cbe70 637 for (int n = FIRST_SAMPLE_INDEX; n <= LAST_SAMPLE_INDEX; n++) {
timmey9 25:abbc19af13f9 638 pc.printf("%i\t%i\t%i\r\n", sample_array1[n], sample_array2[n], angle_array[n]);
timmey9 22:523e316cbe70 639 }
timmey9 22:523e316cbe70 640
timmey9 22:523e316cbe70 641 }