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@29:e6309316c35d, 2014-12-09 (annotated)

Committer:
timmey9
Date:
Tue Dec 09 16:36:50 2014 +0000
Revision:
29:e6309316c35d
Parent:
28:4a833d59897b
Child:
30:1da0bb9c31a6
Uses 'for' loop for sampling.  Sampling looks great and works upto 32000 samples.  Still needs motor control.

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