Same as mallet... but distance
Dependencies: EthernetInterface NetworkAPI mbed-rtos mbed
Fork of MalletFirmware by
main.cpp
- Committer:
- willange
- Date:
- 2014-12-09
- Revision:
- 30:1da0bb9c31a6
- Parent:
- 29:e6309316c35d
File content as of revision 30:1da0bb9c31a6:
// 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 10 // 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 TotalInd(PTC4); DigitalOut SampleInd(PTC5); // 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;} TotalInd = 0; SampleInd = 0; led_blue = 1; led_green = 1; led_red = 1; pc.baud(230400); pc.printf("Starting %s\r\n",NAME); for(int i = 0; i < 86; i++) { if(NVIC_GetPriority((IRQn_Type) i) == 0) NVIC_SetPriority((IRQn_Type) i, 2); } //NVIC_SetPriority(SWI_IRQn,0); //NVIC_SetPriority(Watchdog_IRQn,0); //NVIC_SetPriority(MCM_IRQn,0); //NVIC_SetPriority(PIT0_IRQn,0); //NVIC_SetPriority(PIT1_IRQn,0); //NVIC_SetPriority(PIT2_IRQn,0); NVIC_SetPriority(PIT3_IRQn,0); //NVIC_SetPriority(LPTimer_IRQn,0); NVIC_SetPriority(ADC1_IRQn,0); NVIC_SetPriority(ADC0_IRQn,0); NVIC_SetPriority(ENET_1588_Timer_IRQn,0); NVIC_SetPriority(ENET_Transmit_IRQn,0); NVIC_SetPriority(ENET_Receive_IRQn,0); NVIC_SetPriority(ENET_Error_IRQn,0); // 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); //NVIC_SetPriority(TIMER3_IRQn,0); //pc.printf("Ticker IRQ: %i\r\n", Sampler.irq()); // 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; NVIC_SetPriority(ENET_1588_Timer_IRQn,1); NVIC_SetPriority(ENET_Transmit_IRQn,1); NVIC_SetPriority(ENET_Receive_IRQn,1); NVIC_SetPriority(ENET_Error_IRQn,1); //for(int i = 0; i < 86; i++) pc.printf("%i: %i\r\n", i, NVIC_GetPriority((IRQn_Type) i)); 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 't': { for(int i = 0; i < 86; i++) pc.printf("%i: %i\r\n", i, NVIC_GetPriority((IRQn_Type) i)); } break; case '1': // run motor and sample { 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) client[index].write((void *)"Data\n",5); TotalInd = 1; uint32_t AMT20_AB; rotary_count = 0; __disable_irq(); SampleInd = 0; for(int i = 0; i < TOTAL_SAMPLES; i++) { SampleInd = !SampleInd; //sample_array1[i] = adc_hal_get_conversion_value(0, 0); //sample_array2[i] = 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 AMT20_AB = HW_GPIO_PDIR_RD(HW_PORTC) & 0x03; 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; } angle_array[i] = rotary_count; wait_us(8); } __enable_irq(); NVIC_SetPriority(ENET_1588_Timer_IRQn,0); NVIC_SetPriority(ENET_Transmit_IRQn,0); NVIC_SetPriority(ENET_Receive_IRQn,0); NVIC_SetPriority(ENET_Error_IRQn,0); TotalInd = 1; 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); TotalInd = 0; NVIC_SetPriority(ENET_1588_Timer_IRQn,1); NVIC_SetPriority(ENET_Transmit_IRQn,1); NVIC_SetPriority(ENET_Receive_IRQn,1); NVIC_SetPriority(ENET_Error_IRQn,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; } } } break; } } } while (server.getStatus() == network::Socket::Listening); } void timed_sampling() { SampleInd = 1; pc.printf("TICKER!\n"); //__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 */ SampleInd = 0; } 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);