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@26:a00bf9837e03, 2014-12-03 (annotated)

Committer:
timmey9
Date:
Wed Dec 03 16:40:51 2014 +0000
Revision:
26:a00bf9837e03
Parent:
25:abbc19af13f9
Child:
27:8c2b30c855d1
Kind of works, but probably wrong.

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 26:a00bf9837e03 46 #define TOTAL_SAMPLES 10 // 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 26:a00bf9837e03 360 client[index].write((void *)"Data\n",5);
timmey9 26:a00bf9837e03 361
timmey9 26:a00bf9837e03 362 for(int i = 0; i < 10000; i++)
timmey9 25:abbc19af13f9 363 {
timmey9 25:abbc19af13f9 364 StatusIndicator2 = !StatusIndicator2;
timmey9 25:abbc19af13f9 365
timmey9 25:abbc19af13f9 366 uint32_t A0_value = adc_hal_get_conversion_value(0, 0);
timmey9 25:abbc19af13f9 367 uint32_t A2_value = adc_hal_get_conversion_value(1, 0);
timmey9 25:abbc19af13f9 368 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 369 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 370
timmey9 25:abbc19af13f9 371 // The following updates the rotary counter for the AMT20 sensor
timmey9 25:abbc19af13f9 372 // Put A on PTC0
timmey9 25:abbc19af13f9 373 // Put B on PTC1
timmey9 25:abbc19af13f9 374 uint32_t AMT20_AB = HW_GPIO_PDIR_RD(HW_PORTC) & 0x03;
timmey9 25:abbc19af13f9 375 //AMT20_AB = ~last_AMT20_AB_read; // Used to force a count - extend time.
timmey9 25:abbc19af13f9 376 if (AMT20_AB != last_AMT20_AB_read)
timmey9 25:abbc19af13f9 377 {
timmey9 25:abbc19af13f9 378 // change "INVERT_ANGLE" to change whether relative angle counts up or down.
timmey9 25:abbc19af13f9 379 if ((AMT20_AB >> 1)^(last_AMT20_AB_read) & 1U)
timmey9 25:abbc19af13f9 380 #if INVERT_ANGLE == 1
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 else
timmey9 25:abbc19af13f9 387 {rotary_count--;}
timmey9 25:abbc19af13f9 388 #endif
timmey9 25:abbc19af13f9 389
timmey9 25:abbc19af13f9 390 last_AMT20_AB_read = AMT20_AB;
timmey9 25:abbc19af13f9 391 }
timmey9 25:abbc19af13f9 392
timmey9 26:a00bf9837e03 393
timmey9 26:a00bf9837e03 394 char buf[2];
timmey9 26:a00bf9837e03 395 buf[1] = (A0_value >> 8) & 0xff;
timmey9 26:a00bf9837e03 396 buf[0] = A0_value & 0xff;
timmey9 26:a00bf9837e03 397 client[index].write((void *)buf,2);
timmey9 26:a00bf9837e03 398 buf[1] = (A2_value >> 8) & 0xff;
timmey9 26:a00bf9837e03 399 buf[0] = A2_value & 0xff;
timmey9 26:a00bf9837e03 400 client[index].write((void *)buf,2);
timmey9 26:a00bf9837e03 401 buf[1] = (rotary_count >> 8) & 0xff;
timmey9 26:a00bf9837e03 402 buf[0] = rotary_count & 0xff;
timmey9 26:a00bf9837e03 403 client[index].write((void *)buf,2);
timmey9 26:a00bf9837e03 404
timmey9 26:a00bf9837e03 405 //sample_array1[i] = A0_value;
timmey9 26:a00bf9837e03 406 //sample_array2[i] = A2_value;
timmey9 26:a00bf9837e03 407 //angle_array[i] = rotary_count;
timmey9 25:abbc19af13f9 408
timmey9 25:abbc19af13f9 409 wait_us(10);
timmey9 25:abbc19af13f9 410 }
timmey9 26:a00bf9837e03 411
timmey9 26:a00bf9837e03 412 //client[index].write((void *)&sample_array1,2*TOTAL_SAMPLES);
timmey9 26:a00bf9837e03 413 //client[index].write((void *)&sample_array2,2*TOTAL_SAMPLES);
timmey9 26:a00bf9837e03 414 //client[index].write((void *)&angle_array,2*TOTAL_SAMPLES);
timmey9 25:abbc19af13f9 415
timmey9 25:abbc19af13f9 416 //above is a crappy form of sampling ********************************
timmey9 25:abbc19af13f9 417
timmey9 25:abbc19af13f9 418 //__disable_irq(); // Disable Interrupts
timmey9 25:abbc19af13f9 419
timmey9 25:abbc19af13f9 420 // reset mallet
timmey9 25:abbc19af13f9 421 // add code here
timmey9 25:abbc19af13f9 422
timmey9 25:abbc19af13f9 423 output_data(trigger_count);
timmey9 25:abbc19af13f9 424 }
timmey9 24:a5891669afc5 425 break;
timmey9 25:abbc19af13f9 426
timmey9 25:abbc19af13f9 427 case '2': // run just the motor
timmey9 25:abbc19af13f9 428 {
timmey9 25:abbc19af13f9 429 pc.printf("All duration settings 2:\r\n");
timmey9 25:abbc19af13f9 430 for(int i = 0; i < 8; i++)
timmey9 25:abbc19af13f9 431 {
timmey9 25:abbc19af13f9 432 pc.printf("Duration[%i]: %i\r\n", i, duration[i]);
timmey9 25:abbc19af13f9 433 }
timmey9 25:abbc19af13f9 434
timmey9 25:abbc19af13f9 435 // release mallet
timmey9 25:abbc19af13f9 436 motor.forward(duration[0]); // move motor forward
timmey9 25:abbc19af13f9 437 wait_us(duration[1]); // wait
timmey9 25:abbc19af13f9 438 motor.backward(0.7, duration[2]); // stop motor using reverse
timmey9 25:abbc19af13f9 439
timmey9 25:abbc19af13f9 440 // time for sampling
timmey9 25:abbc19af13f9 441 wait_us(SAMPLING_RATE*TOTAL_SAMPLES);
timmey9 25:abbc19af13f9 442
timmey9 25:abbc19af13f9 443 // reset mallet
timmey9 25:abbc19af13f9 444 motor.backward(duration[3]); // move motor backward
timmey9 25:abbc19af13f9 445 motor.backward(0.75, duration[4]);
timmey9 25:abbc19af13f9 446 motor.backward(duty_cycle, duration[5]);
timmey9 25:abbc19af13f9 447 }
timmey9 24:a5891669afc5 448 break;
timmey9 24:a5891669afc5 449 case 'a':
timmey9 24:a5891669afc5 450 if(angle_encoder.set_zero(&rotary_count)) {
timmey9 24:a5891669afc5 451 client[index].write((void *) "Zero set\n",9);
timmey9 24:a5891669afc5 452 }
timmey9 24:a5891669afc5 453 else {
timmey9 24:a5891669afc5 454 client[index].write((void *) "Zero NOT set\n",13);
timmey9 24:a5891669afc5 455 }
timmey9 24:a5891669afc5 456 break;
timmey9 24:a5891669afc5 457 case 's':
timmey9 24:a5891669afc5 458 {
timmey9 24:a5891669afc5 459 char buf[16];
timmey9 24:a5891669afc5 460 sprintf(buf,"NOP: %x\n",angle_encoder.nop());
timmey9 24:a5891669afc5 461 client[index].write((void *) buf,16);
timmey9 24:a5891669afc5 462 break;
timmey9 24:a5891669afc5 463 }
timmey9 24:a5891669afc5 464 case 'd':
timmey9 24:a5891669afc5 465 {
timmey9 24:a5891669afc5 466 char buf[29];
timmey9 24:a5891669afc5 467 sprintf(buf,"Angle: %i %i\n",angle_encoder.absolute_angle(), rotary_count);
timmey9 24:a5891669afc5 468 client[index].write((void *) buf,29);
timmey9 24:a5891669afc5 469 break;
timmey9 24:a5891669afc5 470 }
timmey9 24:a5891669afc5 471 /*
timmey9 24:a5891669afc5 472
timmey9 24:a5891669afc5 473 // for motor testing
timmey9 24:a5891669afc5 474 if(temp == 'k') // motor backward
timmey9 24:a5891669afc5 475 {
timmey9 24:a5891669afc5 476 motor.backward(duty_cycle, duration[5]);
timmey9 24:a5891669afc5 477 }
timmey9 24:a5891669afc5 478
timmey9 24:a5891669afc5 479 if(temp == 'u' && duty_cycle < 1.00f) pc.printf("%f \r\n", duty_cycle += 0.01f);
timmey9 24:a5891669afc5 480 if(temp == 'i' && duty_cycle > 0.00f) pc.printf("%f \r\n", duty_cycle -= 0.01f);
timmey9 24:a5891669afc5 481
timmey9 24:a5891669afc5 482 if(temp == '=') // you can hit the '+' key to increment "duration" without holding down "shift"
timmey9 24:a5891669afc5 483 {
timmey9 24:a5891669afc5 484 if(pointer < 7) pointer++;
timmey9 24:a5891669afc5 485 pc.printf("Duration[%i]: %i\r\n",pointer, duration[pointer]);
timmey9 24:a5891669afc5 486 }
timmey9 24:a5891669afc5 487 if(temp == '-')
timmey9 24:a5891669afc5 488 {
timmey9 24:a5891669afc5 489 if(pointer > 0) pointer--;
timmey9 24:a5891669afc5 490 pc.printf("Duration[%i]: %i\r\n",pointer, duration[pointer]);
timmey9 24:a5891669afc5 491 }
timmey9 24:a5891669afc5 492 if(temp == ']') // you can hit the '+' key to increment "duration" without holding down "shift"
timmey9 24:a5891669afc5 493 {
timmey9 24:a5891669afc5 494 duration[pointer] += 1000;
timmey9 24:a5891669afc5 495 pc.printf(" %i\r\n", duration[pointer]);
timmey9 24:a5891669afc5 496 }
timmey9 24:a5891669afc5 497 if(temp == '[')
timmey9 24:a5891669afc5 498 {
timmey9 24:a5891669afc5 499 if(duration[pointer] > 0) duration[pointer]-= 1000;
timmey9 24:a5891669afc5 500 pc.printf(" %i\r\n", duration[pointer]);
timmey9 24:a5891669afc5 501 }
timmey9 24:a5891669afc5 502
timmey9 24:a5891669afc5 503 }
timmey9 24:a5891669afc5 504
timmey9 24:a5891669afc5 505 */
timmey9 24:a5891669afc5 506
timmey9 24:a5891669afc5 507
timmey9 24:a5891669afc5 508
timmey9 21:1fb5023b72af 509 }
timmey9 21:1fb5023b72af 510 }
timmey9 21:1fb5023b72af 511
timmey9 21:1fb5023b72af 512
timmey9 20:f533b3c9296f 513
timmey9 20:f533b3c9296f 514 //***************** print a message back to the client
timmey9 21:1fb5023b72af 515
timmey9 21:1fb5023b72af 516 //client[index].write((void *)&sample_array,SAMPLES);
timmey9 21:1fb5023b72af 517
timmey9 21:1fb5023b72af 518
timmey9 21:1fb5023b72af 519
timmey9 20:f533b3c9296f 520
timmey9 21:1fb5023b72af 521 /*for(int i = 1; i <= SAMPLES+1;)
timmey9 21:1fb5023b72af 522 {
timmey9 21:1fb5023b72af 523 for(int j = 0; j < 20; j++)
timmey9 21:1fb5023b72af 524 {
timmey9 21:1fb5023b72af 525 Timer timeStamp;
timmey9 21:1fb5023b72af 526 timeStamp.stop();
timmey9 21:1fb5023b72af 527 timeStamp.reset();
timmey9 21:1fb5023b72af 528 timeStamp.start();
timmey9 21:1fb5023b72af 529
timmey9 21:1fb5023b72af 530 client[index].write((void *)&sample_array,i);
timmey9 21:1fb5023b72af 531 int timeStampVar = timeStamp.read_us();
timmey9 21:1fb5023b72af 532 timeStamp.stop();
timmey9 21:1fb5023b72af 533
timmey9 21:1fb5023b72af 534 pc.printf("*******\r\n%i\r\nTime taken to send data: %i\r\n", i,timeStampVar);
timmey9 21:1fb5023b72af 535
timmey9 21:1fb5023b72af 536 char premessage[40];
timmey9 21:1fb5023b72af 537 sprintf(premessage, "******\r\n%i\r\nTime taken to send data: %i\r\n", i, timeStampVar);
timmey9 21:1fb5023b72af 538 std::string response1 = premessage;
timmey9 21:1fb5023b72af 539 client[index].write((void *)response1.data(), response1.size());
timmey9 21:1fb5023b72af 540 wait_us(5000);
timmey9 21:1fb5023b72af 541 }
timmey9 21:1fb5023b72af 542 if(i == 10000) i = SAMPLES;
timmey9 21:1fb5023b72af 543 else if(i == SAMPLES) i = i*10;
timmey9 21:1fb5023b72af 544 else i = i*10;
timmey9 21:1fb5023b72af 545 }
timmey9 21:1fb5023b72af 546 std::string endMessage("end");
timmey9 21:1fb5023b72af 547 client[index].write((void *)endMessage.data(), endMessage.size());
timmey9 21:1fb5023b72af 548 */
timmey9 17:2f978f823020 549 break;
timmey9 17:2f978f823020 550 }
timmey9 16:c3f922f61b8f 551 }
timmey9 17:2f978f823020 552
timmey9 17:2f978f823020 553 } while (server.getStatus() == network::Socket::Listening);
timmey9 22:523e316cbe70 554 }
timmey9 23:9e5141647775 555
timmey9 22:523e316cbe70 556 void timed_sampling() {
timmey9 22:523e316cbe70 557 __disable_irq(); // Disable Interrupts
timmey9 23:9e5141647775 558 //timeStamp.start();
timmey9 22:523e316cbe70 559
timmey9 22:523e316cbe70 560 // The following performs analog-to-digital conversions - first reading the last conversion - then initiating another
timmey9 22:523e316cbe70 561 uint32_t A0_value = adc_hal_get_conversion_value(0, 0);
timmey9 22:523e316cbe70 562 uint32_t A2_value = adc_hal_get_conversion_value(1, 0);
timmey9 22:523e316cbe70 563 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 564 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 565
timmey9 22:523e316cbe70 566 // The following updates the rotary counter for the AMT20 sensor
timmey9 22:523e316cbe70 567 // Put A on PTC0
timmey9 22:523e316cbe70 568 // Put B on PTC1
timmey9 22:523e316cbe70 569 uint32_t AMT20_AB = HW_GPIO_PDIR_RD(HW_PORTC) & 0x03;
timmey9 22:523e316cbe70 570 //AMT20_AB = ~last_AMT20_AB_read; // Used to force a count - extend time.
timmey9 22:523e316cbe70 571 if (AMT20_AB != last_AMT20_AB_read)
timmey9 22:523e316cbe70 572 {
timmey9 22:523e316cbe70 573 // change "INVERT_ANGLE" to change whether relative angle counts up or down.
timmey9 22:523e316cbe70 574 if ((AMT20_AB >> 1)^(last_AMT20_AB_read) & 1U)
timmey9 22:523e316cbe70 575 #if INVERT_ANGLE == 1
timmey9 22:523e316cbe70 576 {rotary_count--;}
timmey9 22:523e316cbe70 577 else
timmey9 22:523e316cbe70 578 {rotary_count++;}
timmey9 22:523e316cbe70 579 #else
timmey9 22:523e316cbe70 580 {rotary_count++;}
timmey9 22:523e316cbe70 581 else
timmey9 22:523e316cbe70 582 {rotary_count--;}
timmey9 22:523e316cbe70 583 #endif
timmey9 22:523e316cbe70 584
timmey9 22:523e316cbe70 585 last_AMT20_AB_read = AMT20_AB;
timmey9 22:523e316cbe70 586 }
timmey9 22:523e316cbe70 587 //current_sample_index = BEGIN_SAMPLING; // Used to force extra time.
timmey9 22:523e316cbe70 588 if (current_sample_index == WAITING_TO_BEGIN) {}
timmey9 22:523e316cbe70 589 else
timmey9 22:523e316cbe70 590 {
timmey9 22:523e316cbe70 591 if (current_sample_index == BEGIN_SAMPLING) {
timmey9 22:523e316cbe70 592 current_sample_index = FIRST_SAMPLE_INDEX;
timmey9 22:523e316cbe70 593 }
timmey9 22:523e316cbe70 594
timmey9 25:abbc19af13f9 595 sample_array1[current_sample_index] = (A0_value << CHANNEL_STORAGE_OFFSET) | A2_value;
timmey9 22:523e316cbe70 596
timmey9 22:523e316cbe70 597 angle_array[current_sample_index] = rotary_count;
timmey9 22:523e316cbe70 598
timmey9 22:523e316cbe70 599 if (current_sample_index == LAST_SAMPLE_INDEX) {
timmey9 22:523e316cbe70 600 current_sample_index = WAITING_TO_BEGIN;
timmey9 22:523e316cbe70 601 //led_green = 1;
timmey9 22:523e316cbe70 602 }
timmey9 22:523e316cbe70 603 else { current_sample_index++; }
timmey9 22:523e316cbe70 604 }
timmey9 22:523e316cbe70 605
timmey9 23:9e5141647775 606 //int tempVar = timeStamp.read_us();
timmey9 23:9e5141647775 607 //timeStamp.stop();
timmey9 23:9e5141647775 608 //timeStamp.reset();
timmey9 23:9e5141647775 609 //pc.printf("TimeStamp: %i\r\n", tempVar);
timmey9 22:523e316cbe70 610 __enable_irq(); // Enable Interrupts
timmey9 22:523e316cbe70 611 }
timmey9 22:523e316cbe70 612
timmey9 22:523e316cbe70 613 void analog_initialization(PinName pin)
timmey9 22:523e316cbe70 614 {
timmey9 22:523e316cbe70 615 ADCName adc = (ADCName)pinmap_peripheral(pin, PinMap_ADC);
timmey9 22:523e316cbe70 616 // MBED_ASSERT(adc != (ADCName)NC);
timmey9 22:523e316cbe70 617
timmey9 22:523e316cbe70 618 uint32_t instance = adc >> ADC_INSTANCE_SHIFT;
timmey9 22:523e316cbe70 619
timmey9 22:523e316cbe70 620 clock_manager_set_gate(kClockModuleADC, instance, true);
timmey9 22:523e316cbe70 621
timmey9 22:523e316cbe70 622 uint32_t bus_clock;
timmey9 22:523e316cbe70 623 clock_manager_get_frequency(kBusClock, &bus_clock);
timmey9 22:523e316cbe70 624 uint32_t clkdiv;
timmey9 22:523e316cbe70 625 for (clkdiv = 0; clkdiv < 4; clkdiv++) {
timmey9 22:523e316cbe70 626 if ((bus_clock >> clkdiv) <= MAX_FADC)
timmey9 22:523e316cbe70 627 break;
timmey9 22:523e316cbe70 628 }
timmey9 22:523e316cbe70 629 if (clkdiv == 4) {
timmey9 22:523e316cbe70 630 clkdiv = 0x7; //Set max div
timmey9 22:523e316cbe70 631 }
timmey9 22:523e316cbe70 632 // adc is enabled/triggered when reading.
timmey9 22:523e316cbe70 633 adc_hal_set_clock_source_mode(instance, (adc_clock_source_mode_t)(clkdiv >> 2));
timmey9 22:523e316cbe70 634 adc_hal_set_clock_divider_mode(instance, (adc_clock_divider_mode_t)(clkdiv & 0x3));
timmey9 22:523e316cbe70 635 adc_hal_set_reference_voltage_mode(instance, kAdcVoltageVref);
timmey9 22:523e316cbe70 636 adc_hal_set_resolution_mode(instance, kAdcSingleDiff16);
timmey9 22:523e316cbe70 637 adc_hal_configure_continuous_conversion(instance, false);
timmey9 22:523e316cbe70 638 adc_hal_configure_hw_trigger(instance, false); // sw trigger
timmey9 22:523e316cbe70 639 adc_hal_configure_hw_average(instance, false);
timmey9 22:523e316cbe70 640 adc_hal_set_hw_average_mode(instance, kAdcHwAverageCount4);
timmey9 22:523e316cbe70 641 adc_hal_set_group_mux(instance, kAdcChannelMuxB); // only B channels are avail
timmey9 22:523e316cbe70 642
timmey9 22:523e316cbe70 643 pinmap_pinout(pin, PinMap_ADC);
timmey9 22:523e316cbe70 644 }
timmey9 22:523e316cbe70 645
timmey9 22:523e316cbe70 646 void output_data(uint32_t iteration_number)
timmey9 22:523e316cbe70 647 {
timmey9 22:523e316cbe70 648 pc.printf("Iteration: %i\n\r", iteration_number);
timmey9 22:523e316cbe70 649 pc.printf("Sampling rate: %i\n\r", SAMPLING_RATE);
timmey9 22:523e316cbe70 650 pc.printf("Data length: %i\n\r", TOTAL_SAMPLES);
timmey9 22:523e316cbe70 651 for (int n = FIRST_SAMPLE_INDEX; n <= LAST_SAMPLE_INDEX; n++) {
timmey9 25:abbc19af13f9 652 pc.printf("%i\t%i\t%i\r\n", sample_array1[n], sample_array2[n], angle_array[n]);
timmey9 22:523e316cbe70 653 }
timmey9 22:523e316cbe70 654
timmey9 22:523e316cbe70 655 }