File content as of revision 27:8c2b30c855d1:

// Server code
#include "mbed.h"
#include <stdio.h>

// Ethernet
#include "EthernetInterface.h"
#include "NetworkAPI/buffer.hpp"
#include "NetworkAPI/select.hpp"
#include "NetworkAPI/ip/address.hpp"
#include "NetworkAPI/tcp/socket.hpp"

// Angle encoder and motor control
#include "AngleEncoder.h"
#include "MotorControl.h"

// Analog sampling
#include "PeripheralNames.h"
#include "PeripheralPins.h"
#include "fsl_adc_hal.h"
#include "fsl_clock_manager.h"
#include "fsl_dspi_hal.h"
#include "AngleEncoder.h"

/*****************************************
 *  
 *   Configuration
 *
 *   Take the time to set these constants
 *
 *****************************************/
#define MALLET 6        // set mallet to a value between 1-7
#define STATIC 1        // set STATIC to 1 for static ip, set STATIC to 0 for dynamic
#define PORT 22         // set to a random port number.  All the mallets can use the same port number.
#define MAX_CLIENTS 2   // set the max number of clients to at least 2 (first client is MATLAB, second is the distance unit)
#define INVERT_ANGLE 0  // inverts whether the angle encoder counts up or down

// Analog sampling
#define MAX_FADC 6000000
#define SAMPLING_RATE       10000 // In microseconds, so 10 us will be a sampling rate of 100 kHz
#define TOTAL_SAMPLES       30000 // originally 30000 for 0.3 ms of sampling.

#define LAST_SAMPLE_INDEX   (TOTAL_SAMPLES-1) // If sampling time is 25 us, then 2000 corresponds to 50 ms
#define FIRST_SAMPLE_INDEX  0
#define BEGIN_SAMPLING      0xFFFFFFFF
#define WAITING_TO_BEGIN    (BEGIN_SAMPLING-1)

#define CHANNEL_STORAGE_OFFSET 16 // For storing the 16 bits and the 16 bits in a single 32 bit array
#define PERIOD 3000 // make sure PERIOD >= ON_OFF_TIME
#define ON_OFF_TIME 300 // time it takes for relay to turn on


// Ethernet
#define GATEWAY "169.254.225.1"
#define MASK "255.255.0.0"

// used for assign different mallets their ip addresses
#if MALLET == 1
#define IP "169.254.225.206"
#define NAME "Mallet1\n\r"

#elif MALLET == 2
#define IP "169.254.225.207"
#define NAME "Mallet2\n\r"

#elif MALLET == 3
#define IP "169.254.225.208"
#define NAME "Mallet3\n\r"

#elif MALLET == 4
#define IP "169.254.225.209"
#define NAME "Mallet4\n\r"

#elif MALLET == 5
#define IP "169.254.225.210"
#define NAME "Mallet5\n\r"

#elif MALLET == 6
#define IP "169.254.225.211"
#define NAME "Mallet6\n\r"

#elif MALLET == 7
#define IP "169.254.225.212"
#define NAME "Mallet7\n\r"

#endif


// for debug purposes
Serial pc(USBTX, USBRX);
DigitalOut led_red(LED_RED);
DigitalOut led_green(LED_GREEN);
DigitalOut led_blue(LED_BLUE);

// motor control and angle encoder
MotorControl motor(PTC2, PTA2, PERIOD, ON_OFF_TIME); // forward, backward, period, safetyPeriod
AngleEncoder angle_encoder(PTD2, PTD3, PTD1, PTD0, 8, 0, 1000000); // mosi, miso, sclk, cs, bit_width, mode, hz
DigitalIn AMT20_A(PTC0); // input for quadrature encoding from angle encoder
DigitalIn AMT20_B(PTC1); // input for quadrature encoding from angle encoder

// Analog sampling
Ticker Sampler;
//Timer timer;
//Timer timeStamp;
AnalogIn A0_pin(A0);
AnalogIn A2_pin(A2);

//DigitalIn SW3_switch(PTA4);
//DigitalIn SW2_switch(PTC6);
DigitalOut StatusSensor(PTC4);
DigitalOut StatusIndicator2(PTA0); // originally PTD0 but needed for CS for spi

uint32_t current_sample_index = WAITING_TO_BEGIN;
uint16_t sample_array1[TOTAL_SAMPLES];
uint16_t sample_array2[TOTAL_SAMPLES];
uint16_t angle_array[TOTAL_SAMPLES];

// Declaration of functions
void analog_initialization(PinName pin);
void output_data(uint32_t iteration_number);
void timed_sampling();

// Important globabl variables necessary for the sampling every interval
int rotary_count = 0;
uint32_t last_AMT20_AB_read = 0;

using namespace std;
 
int main() {
    
    //for(int i = 0; i < TOTAL_SAMPLES; i++) {sample_array[i] = i;}
    led_blue = 1;
    led_green = 1;
    led_red = 1;
    
    pc.baud(230400);
    pc.printf("Starting %s\r\n",NAME);
    
    // The ethernet setup must be within the first few lines of code, otherwise the program hangs
    EthernetInterface interface;
    #if STATIC == 1
    interface.init(IP, MASK, GATEWAY);
    #else
    interface.init();
    #endif
    interface.connect();
    pc.printf("IP Address is: %s\n\r", interface.getIPAddress());
    pc.printf("Network Mask is: %s\n\r", interface.getNetworkMask());
    pc.printf("MAC address is: %s\n\r", interface.getMACAddress());
    pc.printf("Gateway is: %s\n\r", interface.getGateway());
    pc.printf("Port is: %i\n\r", PORT);
    
    // ethernet setup failed for some reason.  Flash yellow light then uC resets itself
    if(interface.getIPAddress() == 0)
    {
        for(int i = 0; i < 5; i++)
        {
            led_red = 0;
            led_green = 0;
            wait_ms(500);
            led_red = 1;
            led_green = 1;
            wait_ms(1000);
        }
        NVIC_SystemReset();
    }
    
    
    analog_initialization(A0);
    analog_initialization(A2);
    
    
    // Start the sampling loop
    current_sample_index = WAITING_TO_BEGIN;
    Sampler.attach_us(&timed_sampling, SAMPLING_RATE);
    
    
    uint32_t trigger_count = 0;
    
    // corresponding duty   1  0  0.7 1  0.75 
    uint32_t duration[8] = {0, 0,  0, 0,  0,  0,  0,  0};
    
    double duty_cycle = 0.25;
    
    network::Select select;
    network::tcp::Socket server;
    network::tcp::Socket client[MAX_CLIENTS];
    network::tcp::Socket *socket = NULL;
     
    int result = 0;
    int index = 0;
     
    network::Buffer buffer(50);
    std::string message(NAME);
     
    // Configure the server socket (assume every thing works)
    server.open();
    server.bind(PORT);
    server.listen(MAX_CLIENTS);
    
    // Add sockets to the select api
    select.set(&server, network::Select::Read);
    for (index = 0; index < MAX_CLIENTS; index++) {
        select.set(&client[index], network::Select::Read);
    }
    
    led_red = 1;
    led_green = 1;
    led_blue = 0;
    wait_ms(500);
    led_blue = 1;
    wait_ms(200);
    led_blue = 0;
    wait_ms(500);
    led_blue = 1;
    
    do {
        // Wait for activity
        result = select.wait();
        if (result < -1) {
            pc.printf("Failed to select\n\r");
            break;
        }
         
        // Get the first socket
        socket = (network::tcp::Socket *)select.getReadable();
         
        for (; socket != NULL; socket = (network::tcp::Socket *)select.getReadable()) {
            // Check if there was a connection request.
            if (socket->getHandle() == server.getHandle()) {                
                // Find an unused client
                for (index = 0; index < MAX_CLIENTS; index++) {
                    if (client[index].getStatus() == network::Socket::Closed) {
                        break;
                    }
                }
                 
                // Maximum connections reached
                if (index == MAX_CLIENTS) {
                    pc.printf("Maximum connections reached\n\r");
                    wait(1);
                    continue;
                }
                
                // Accept the client
                socket->accept(client[index]);
                pc.printf("Client connected %s:%d\n\r",
                    client[index].getRemoteEndpoint().getAddress().toString().c_str(),
                    client[index].getRemoteEndpoint().getPort());
                     
                // Send a nice message to the client (tell MATLAB your name
                client[index].write((void *)message.data(), message.size());
                
                continue;
            }
            
            // It was not the server socket, so it must be a client talking to us.
            switch (socket->read(buffer)) {
                case 0:
                    // Remote end disconnected
                    pc.printf("Client disconnected %s:%d\n\r",
                        socket->getRemoteEndpoint().getAddress().toString().c_str(),
                        socket->getRemoteEndpoint().getPort());
                     
                    // Close socket
                    socket->close();
                    break;
                
                case -1:
                    pc.printf("Error while reading data from socket\n\r");
                    socket->close();
                    break;
//************* this is where data is printed to the screen
                default:
                    pc.printf("Message from %s:%d\n\r",
                        socket->getRemoteEndpoint().getAddress().toString().c_str(),
                        socket->getRemoteEndpoint().getPort());
                         
                    pc.printf("%s\n\r", (char *)buffer.data());
                    
                    // read first character for command
                    char command[2];
                    buffer.read(command,2,0);
                    if(command[1] == ':') {
                        switch(command[0])
                        {
                            case 'b':
                                led_blue = !led_blue;
                                client[index].write((void *)"Blue LED\n",9);
                                break;
                                
                            case 'r':
                                led_red = !led_red;
                                client[index].write((void *)"Red LED\n",8);
                                break;
                                
                            case 'p':
                                led_green = !led_green;
                                client[index].write((void *)"Data\n",5);
                                for(int i = 0; i < 99; i++) sample_array1[i] = i;
                                client[index].write((void *)&sample_array1,2*99);
                                break;
                                
                            case '1': // run motor and sample
                            {
                                led_green = 0;
                                client[index].write((void *)"Data\n",5);
                                current_sample_index = BEGIN_SAMPLING;
                                trigger_count++;
                                while (current_sample_index != WAITING_TO_BEGIN){
                                    wait_us(10);
                                    }
                                led_green = 1;
                                led_blue = 0;
                                
                                // send samples all at once
                                client[index].write((void *)&sample_array1,2*TOTAL_SAMPLES);
                                client[index].write((void *)&sample_array2,2*TOTAL_SAMPLES);
                                client[index].write((void *)&angle_array,2*TOTAL_SAMPLES);                                
                                led_blue = 1;
                                }
                                break;
                                
                            case '2': // run just the motor
                            {
                                pc.printf("All duration settings 2:\r\n");
                                for(int i = 0; i < 8; i++)
                                {
                                    pc.printf("Duration[%i]: %i\r\n", i, duration[i]);
                                    }
                                
                                // release mallet
                                motor.forward(duration[0]); // move motor forward
                                wait_us(duration[1]); // wait
                                motor.backward(0.7, duration[2]); // stop motor using reverse
                                
                                // time for sampling
                                wait_us(SAMPLING_RATE*TOTAL_SAMPLES);
                                
                                // reset mallet
                                motor.backward(duration[3]);    // move motor backward
                                motor.backward(0.75, duration[4]);
                                motor.backward(duty_cycle, duration[5]);
                                }
                                break;
                            case 'a':
                                if(angle_encoder.set_zero(&rotary_count)) {
                                    client[index].write((void *) "Zero set\n",9);
                                    }
                                else {
                                    client[index].write((void *) "Zero NOT set\n",13);
                                    }
                                break;
                            case 's':
                            {
                                char buf[16];
                                sprintf(buf,"NOP: %x\n",angle_encoder.nop());
                                client[index].write((void *) buf,16);
                                break;
                                }
                            case 'd':
                            {
                                char buf[29];
                                sprintf(buf,"Angle: %i %i\n",angle_encoder.absolute_angle(), rotary_count);
                                client[index].write((void *) buf,29);
                                break;
                                }
                            /*    
                                
                                // for motor testing
                                if(temp == 'k') // motor backward
                                {
                                    motor.backward(duty_cycle, duration[5]);
                                    }
                                
                                if(temp == 'u' && duty_cycle < 1.00f) pc.printf("%f \r\n", duty_cycle += 0.01f);
                                if(temp == 'i' && duty_cycle > 0.00f) pc.printf("%f \r\n", duty_cycle -= 0.01f);
                                
                                if(temp == '=') // you can hit the '+' key to increment "duration" without holding down "shift"
                                {
                                    if(pointer < 7) pointer++;
                                    pc.printf("Duration[%i]: %i\r\n",pointer, duration[pointer]);
                                }
                                if(temp == '-')
                                {
                                    if(pointer > 0) pointer--;
                                    pc.printf("Duration[%i]: %i\r\n",pointer, duration[pointer]);
                                }
                                if(temp == ']') // you can hit the '+' key to increment "duration" without holding down "shift"
                                {
                                    duration[pointer] += 1000;
                                    pc.printf("             %i\r\n", duration[pointer]);
                                }
                                if(temp == '[')
                                {
                                    if(duration[pointer] > 0) duration[pointer]-= 1000;
                                    pc.printf("             %i\r\n", duration[pointer]);
                                }
                                
                            }
                            
                            */    
                                
                                
                                
                            }
                        }
                    
                    
                    
//***************** print a message back to the client
                    
                    //client[index].write((void *)&sample_array,SAMPLES);
                    
                    
            
                    
                    /*for(int i = 1; i <= SAMPLES+1;)
                    {
                        for(int j = 0; j < 20; j++)
                        {
                            Timer timeStamp;
                            timeStamp.stop();
                            timeStamp.reset();
                            timeStamp.start();
                            
                            client[index].write((void *)&sample_array,i);
                            int timeStampVar = timeStamp.read_us();
                            timeStamp.stop();
    
                            pc.printf("*******\r\n%i\r\nTime taken to send data: %i\r\n", i,timeStampVar);
                            
                            char premessage[40];
                            sprintf(premessage, "******\r\n%i\r\nTime taken to send data: %i\r\n", i, timeStampVar);
                            std::string response1 = premessage;
                            client[index].write((void *)response1.data(), response1.size());
                            wait_us(5000);
                        }
                        if(i == 10000) i = SAMPLES;
                        else if(i == SAMPLES) i = i*10;
                        else i = i*10;
                    }
                    std::string endMessage("end");
                    client[index].write((void *)endMessage.data(), endMessage.size());
                    */
                    break;
            }
        }
             
    } while (server.getStatus() == network::Socket::Listening);
}

void timed_sampling() {
    if(switcher) led_red = !led_red;
    /*
    //__disable_irq();    // Disable Interrupts
    //timeStamp.start();
    
    // The following performs analog-to-digital conversions - first reading the last conversion - then initiating another
    uint32_t A0_value = adc_hal_get_conversion_value(0, 0);
    uint32_t A2_value = adc_hal_get_conversion_value(1, 0);
    BW_ADC_SC1n_ADCH(0, 0, kAdcChannel12);      // This corresponds to starting an ADC conversion on channel 12 of ADC 0 - which is A0 (PTB2)
    BW_ADC_SC1n_ADCH(1, 0, kAdcChannel14);      // This corresponds to starting an ADC conversion on channel 14 of ADC 1 - which is A2 (PTB10)
    
    // The following updates the rotary counter for the AMT20 sensor
    // Put A on PTC0
    // Put B on PTC1
    uint32_t AMT20_AB = HW_GPIO_PDIR_RD(HW_PORTC) & 0x03;
    //AMT20_AB = ~last_AMT20_AB_read; // Used to force a count - extend time.
    if (AMT20_AB != last_AMT20_AB_read)
    {
        // change "INVERT_ANGLE" to change whether relative angle counts up or down.
        if ((AMT20_AB >> 1)^(last_AMT20_AB_read) & 1U)
        #if INVERT_ANGLE == 1
            {rotary_count--;}
        else
            {rotary_count++;}
        #else
            {rotary_count++;}
        else
            {rotary_count--;}
        #endif
        
        last_AMT20_AB_read = AMT20_AB;        
    }
    //current_sample_index = BEGIN_SAMPLING; // Used to force extra time.
    if (current_sample_index == WAITING_TO_BEGIN) {}
    else
        { 
        if (current_sample_index == BEGIN_SAMPLING) {
            current_sample_index = FIRST_SAMPLE_INDEX;
            }
            
        sample_array1[current_sample_index] = A0_value;
        sample_array2[current_sample_index] = A2_value;
        angle_array[current_sample_index] = rotary_count;
        
        if (current_sample_index == LAST_SAMPLE_INDEX) {
            current_sample_index = WAITING_TO_BEGIN;
            }
        else { current_sample_index++; }
        }
    
    //int tempVar = timeStamp.read_us();
    //timeStamp.stop();
    //timeStamp.reset();
    //pc.printf("TimeStamp: %i\r\n", tempVar);
    //__enable_irq();     // Enable Interrupts
    */
}

void analog_initialization(PinName pin)
{
    ADCName adc = (ADCName)pinmap_peripheral(pin, PinMap_ADC);
//    MBED_ASSERT(adc != (ADCName)NC);

    uint32_t instance = adc >> ADC_INSTANCE_SHIFT;

    clock_manager_set_gate(kClockModuleADC, instance, true);

    uint32_t bus_clock;
    clock_manager_get_frequency(kBusClock, &bus_clock);
    uint32_t clkdiv;
    for (clkdiv = 0; clkdiv < 4; clkdiv++) {
        if ((bus_clock >> clkdiv) <= MAX_FADC)
            break;
    }
    if (clkdiv == 4) {
        clkdiv = 0x7; //Set max div
    }
    // adc is enabled/triggered when reading.
    adc_hal_set_clock_source_mode(instance, (adc_clock_source_mode_t)(clkdiv >> 2));
    adc_hal_set_clock_divider_mode(instance, (adc_clock_divider_mode_t)(clkdiv & 0x3));
    adc_hal_set_reference_voltage_mode(instance, kAdcVoltageVref);
    adc_hal_set_resolution_mode(instance, kAdcSingleDiff16);
    adc_hal_configure_continuous_conversion(instance, false);
    adc_hal_configure_hw_trigger(instance, false); // sw trigger 
    adc_hal_configure_hw_average(instance, false);
    adc_hal_set_hw_average_mode(instance, kAdcHwAverageCount4);
    adc_hal_set_group_mux(instance, kAdcChannelMuxB); // only B channels are avail 

    pinmap_pinout(pin, PinMap_ADC);
}

void output_data(uint32_t iteration_number)
{
    pc.printf("Iteration: %i\n\r", iteration_number);
    pc.printf("Sampling rate: %i\n\r", SAMPLING_RATE);
    pc.printf("Data length: %i\n\r", TOTAL_SAMPLES);
    
    //for (int n = FIRST_SAMPLE_INDEX; n <= LAST_SAMPLE_INDEX; n++) {
    //    pc.printf("%i\t%i\t%i\r\n", sample_array1[n], sample_array2[n], angle_array[n]);
    //    }
    
}

// read some registers for some info.
//uint32_t* rcr = (uint32_t*) 0x400C0084;
//uint32_t* ecr = (uint32_t*) 0x400C0024;
//pc.printf("RCR register: %x\r\n", *rcr);
//pc.printf("ECR register: %x\r\n", *ecr);