/* LUFA Library Copyright (C) Dean Camera, 2017. dean [at] fourwalledcubicle [dot] com www.lufa-lib.org */ /* Copyright 2017 Dean Camera (dean [at] fourwalledcubicle [dot] com) Permission to use, copy, modify, distribute, and sell this software and its documentation for any purpose is hereby granted without fee, provided that the above copyright notice appear in all copies and that both that the copyright notice and this permission notice and warranty disclaimer appear in supporting documentation, and that the name of the author not be used in advertising or publicity pertaining to distribution of the software without specific, written prior permission. The author disclaims all warranties with regard to this software, including all implied warranties of merchantability and fitness. In no event shall the author be liable for any special, indirect or consequential damages or any damages whatsoever resulting from loss of use, data or profits, whether in an action of contract, negligence or other tortious action, arising out of or in connection with the use or performance of this software. */ /** \file * * Main source file for the VirtualSerial demo. This file contains the main tasks of * the demo and is responsible for the initial application hardware configuration. */ #include "final.h" #include <../../../../LUFA/Drivers/Peripheral/ADC.h> #define output(directions,pin) (directions |= pin) // set port direction for output #define set(port,pin) (port |= pin) // set port pin #define clear(port,pin) (port &= (~pin)) // clear port pin #define pin_test(pins,pin) (pins & pin) // test for port pin #define bit_test(byte,bit) (byte & (1 << bit)) // test for bit set #define bit_delay_time 8.5 // bit delay for 115200 with overhead #define bit_delay() _delay_us(bit_delay_time) // RS232 bit delay #define half_bit_delay() _delay_us(bit_delay_time/2) // RS232 half bit delay #define char_delay() _delay_ms(10) // char delay #define position_delay() _delay_ms(3) #define PWM_port PORTB #define PWM_direction DDRB #define PWM_pin_1 (1 << PB5) #define PWM_pin_2 (1 << PB6) #define nsamples 10 // number of samples to accumulate #define loop_count 1 #define max_buffer 25 /** LUFA CDC Class driver interface configuration and state information. This structure is * passed to all CDC Class driver functions, so that multiple instances of the same class * within a device can be differentiated from one another. */ USB_ClassInfo_CDC_Device_t VirtualSerial_CDC_Interface = { .Config = { .ControlInterfaceNumber = INTERFACE_ID_CDC_CCI, .DataINEndpoint = { .Address = CDC_TX_EPADDR, .Size = CDC_TXRX_EPSIZE, .Banks = 1, }, .DataOUTEndpoint = { .Address = CDC_RX_EPADDR, .Size = CDC_TXRX_EPSIZE, .Banks = 1, }, .NotificationEndpoint = { .Address = CDC_NOTIFICATION_EPADDR, .Size = CDC_NOTIFICATION_EPSIZE, .Banks = 1, }, }, }; /** Standard file stream for the CDC interface when set up, so that the virtual CDC COM port can be * used like any regular character stream in the C APIs. */ static FILE USBSerialStream; /** Main program entry point. This routine contains the overall program flow, including initial * setup of all components and the main program loop. */ char ReportString[26]; void parse(int *chrs, int *x, int *y) { // if (chrs[6] != 10 & chrs[7]!= 10) { // *x = *y = 0; // return; // } *x = *y = 0; int i = 0, flag = 0; int sgn = 1, t = 0; while(i<20 && chrs[i] != '\n') { if (chrs[i] == '-'){ sgn = -1; } if (chrs[i] == ' '){ *x = sgn * t; flag++; sgn = 1; t = 0; } if (chrs[i] >= '0' && chrs[i] <= '9') { t = t * 10 + chrs[i] - '0'; } chrs[i] = 0; i++; } *y = sgn * t; chrs[i] = 0; } int read_force1(){ // int accum = 0; double accum1 = 0, accum2 = 0; int count = 0; // ADMUX = (0 << REFS1) | (0 << REFS0) // Vcc ref // | (0 << ADLAR) | (1 << MUX4) | (0 << MUX3) | (0 << MUX2) | (0 << MUX1) | (1 << MUX0); // 40x (ADC5 - ADC0) // ADCSRB = (1 << MUX5); ADMUX = (0 << REFS1) | (0 << REFS0) // Vcc ref | (0 << ADLAR) | (0 << MUX4) | (1 << MUX3) | (0 << MUX2) | (0 << MUX1) | (1 << MUX0); // 10x (ADC5 - ADC0) ADCSRB = (1 << MUX5); // 101001 ADCSRA = (1 << ADEN) // enable | (1 << ADPS2) | (0 << ADPS1) | (1 << ADPS0); // prescaler /32 for (count = 0; count < nsamples; ++count) { // // initiate conversion // ADCSRA |= (1 << ADSC); // // wait for completion // while (ADCSRA & (1 << ADSC)) ; // // add result // accum1 += (ADC * 1.0 / nsamples); } return ceil(accum1); // return(ceil(accum1)); } int read_force2(){ double accum1 = 0, accum2 = 0; int count = 0; // ADMUX = (0 << REFS1) | (0 << REFS0) // Vcc ref // | (0 << ADLAR) | (1 << MUX4) | (0 << MUX3) | (1 << MUX2) | (0 << MUX1) | (0 << MUX0); // 40x (ADC4 - ADC1) // ADCSRB = (1 << MUX5); // // 110100 ADMUX = (0 << REFS1) | (0 << REFS0) // Vcc ref | (0 << ADLAR) | (0 << MUX4) | (1 << MUX3) | (1 << MUX2) | (0 << MUX1) | (0 << MUX0); // 10x (ADC4 - ADC1) ADCSRB = (1 << MUX5); // 101100 ADCSRA = (1 << ADEN) // enable | (1 << ADPS2) | (0 << ADPS1) | (1 << ADPS0); // prescaler /32 for (count = 0; count < nsamples; ++count) { // // initiate conversion // ADCSRA |= (1 << ADSC); // // wait for completion // while (ADCSRA & (1 << ADSC)) ; // // add result // accum2 += (ADC * 1.0 / nsamples); } // *force2 = ceil(accum2); return ceil(accum2); } void delay_us(long long n){ while(n--) _delay_us(1); } void servo_move(double pos1, double pos2){ // int t = 1000 + pos * 1000 / 180; // int t = ceil(pos * 2000 / 180); double angle1 = pos1 / 180.0, angle2 = pos2 / 180.0; int t1 = ceil(500 + angle1 * 2000); int t2 = ceil(500 + angle2 * 2000); // int t = 10; OCR1A = t1; OCR1B = t2; // fprintf(&USBSerialStream, "USB: %d\n", t); // set(PWM_port,PWM_pin_1); // delay_us(t); // clear(PWM_port,PWM_pin_1); // delay_us(4000l-t); } // int max(int a, int b){ // return a>b?a:b; // } // int min(int a, int b){ // return ab?a:b; } double min(double a, double b){ return a force_avg){ // delta = max(0.0, force - force_avg - thres_pos) * sensitivity; // } // else{ // delta = min(0.0, force - force_avg + thres_neg) * sensitivity; // } // pos = pos + delta; // pos = max(min(pos, 180.0), 0.0); // if (fabs(force - force_avg2) > tol) { // if (force > force_avg2) { // pos = pos - sensitivity_pos * (force - force_avg2); // } // else{ // pos = pos - sensitivity_neg * (force - force_avg2); // } // pos = max(min(pos, 180.0), 0.0); // } if (fabs(force1 - force_avg1) > tol1) { if (force1 > force_avg1) { pos1 = pos1 - sensitivity_pos1 * (force1 - force_avg1); } else{ pos1 = pos1 - sensitivity_neg1 * (force1 - force_avg1); } pos1 = max(min(pos1, 180.0), 0.0); } if (fabs(force2 - force_avg2) > tol2) { if (force2 > force_avg2) { pos2 = pos2 + 2 * sensitivity_pos2 * (force2 - force_avg2); } else{ pos2 = pos2 + 2 * sensitivity_neg2 * (force2 - force_avg2); } pos2 = max(min(pos2, 180.0), 0.0); } // ADC_GetChannelReading(ADC_REFERENCE_AVCC | ADC_RIGHT_ADJUSTED | ADC_CHANNEL7); // fprintf(&USBSerialStream, "USB force: %d\n", ADC_GetResult()); servo_move(pos1, pos2); if (!(ReceivedByte < 0)) { chrs[i++] = ReceivedByte; // fprintf(&USBSerialStream, "%c", chrs[i-1]); if (ReceivedByte == 10) { parse(chrs, &x, &y); // sensitivity_neg = s_min + (s_max - s_min) * (x - f_min) / (f_max - f_min); // sensitivity_neg = -0.2 / (x + 1e-6); // sensitivity_neg = pow(10.0, k*x); // thres_neg = k * sqrt(x); sensitivity_neg1 = k / sqrt(max(1e-9, -x)); sensitivity_neg1 = max(s_min, min(sensitivity_neg1, s_max)); if (x < -300) { sensitivity_neg1 = 0; } sensitivity_neg2 = k / sqrt(max(1e-9, -y)); sensitivity_neg2 = max(s_min, min(sensitivity_neg2, s_max)); if (y < -300) { sensitivity_neg2 = 0; } int t1 = (int) (pos1 * 10), t2 = (int) (pos2 * 10); // fprintf(&USBSerialStream, "USB force: %d %d\n", force, force); // fprintf(&USBSerialStream, "USB: %d %d %d\n", x, y, pos); fprintf(&USBSerialStream, "%d %d %d %d\n", t1, t2, force1, force2); // fprintf(&USBSerialStream, "%d %d\n", force1, force2); // fputs(chrs, &USBSerialStream); i=0; } } CDC_Device_USBTask(&VirtualSerial_CDC_Interface); USB_USBTask(); } } /** Configures the board hardware and chip peripherals for the demo's functionality. */ void SetupHardware(void) { #if (ARCH == ARCH_AVR8) /* Disable watchdog if enabled by bootloader/fuses */ MCUSR &= ~(1 << WDRF); wdt_disable(); /* Disable clock division */ clock_prescale_set(clock_div_1); #elif (ARCH == ARCH_XMEGA) /* Start the PLL to multiply the 2MHz RC oscillator to 32MHz and switch the CPU core to run from it */ XMEGACLK_StartPLL(CLOCK_SRC_INT_RC2MHZ, 2000000, F_CPU); XMEGACLK_SetCPUClockSource(CLOCK_SRC_PLL); /* Start the 32MHz internal RC oscillator and start the DFLL to increase it to 48MHz using the USB SOF as a reference */ XMEGACLK_StartInternalOscillator(CLOCK_SRC_INT_RC32MHZ); XMEGACLK_StartDFLL(CLOCK_SRC_INT_RC32MHZ, DFLL_REF_INT_USBSOF, F_USB); PMIC.CTRL = PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm; #endif /* Hardware Initialization */ Joystick_Init(); LEDs_Init(); USB_Init(); } /** Checks for changes in the position of the board joystick, sending strings to the host upon each change. */ void CheckJoystickMovement(void) { uint8_t JoyStatus_LCL = Joystick_GetStatus(); char* ReportString = NULL; static bool ActionSent = false; if (JoyStatus_LCL & JOY_UP) ReportString = "Joystick Up\r\n"; else if (JoyStatus_LCL & JOY_DOWN) ReportString = "Joystick Down\r\n"; else if (JoyStatus_LCL & JOY_LEFT) ReportString = "Joystick Left\r\n"; else if (JoyStatus_LCL & JOY_RIGHT) ReportString = "Joystick Right\r\n"; else if (JoyStatus_LCL & JOY_PRESS) ReportString = "Joystick Pressed\r\n"; else ActionSent = false; if ((ReportString != NULL) && (ActionSent == false)) { ActionSent = true; /* Write the string to the virtual COM port via the created character stream */ fputs(ReportString, &USBSerialStream); /* Alternatively, without the stream: */ // CDC_Device_SendString(&VirtualSerial_CDC_Interface, ReportString); } } /** Event handler for the library USB Connection event. */ void EVENT_USB_Device_Connect(void) { LEDs_SetAllLEDs(LEDMASK_USB_ENUMERATING); } /** Event handler for the library USB Disconnection event. */ void EVENT_USB_Device_Disconnect(void) { LEDs_SetAllLEDs(LEDMASK_USB_NOTREADY); } /** Event handler for the library USB Configuration Changed event. */ void EVENT_USB_Device_ConfigurationChanged(void) { bool ConfigSuccess = true; ConfigSuccess &= CDC_Device_ConfigureEndpoints(&VirtualSerial_CDC_Interface); LEDs_SetAllLEDs(ConfigSuccess ? LEDMASK_USB_READY : LEDMASK_USB_ERROR); } /** Event handler for the library USB Control Request reception event. */ void EVENT_USB_Device_ControlRequest(void) { CDC_Device_ProcessControlRequest(&VirtualSerial_CDC_Interface); } /** CDC class driver callback function the processing of changes to the virtual * control lines sent from the host.. * * \param[in] CDCInterfaceInfo Pointer to the CDC class interface configuration structure being referenced */ void EVENT_CDC_Device_ControLineStateChanged(USB_ClassInfo_CDC_Device_t *const CDCInterfaceInfo) { /* You can get changes to the virtual CDC lines in this callback; a common use-case is to use the Data Terminal Ready (DTR) flag to enable and disable CDC communications in your application when set to avoid the application blocking while waiting for a host to become ready and read in the pending data from the USB endpoints. */ bool HostReady = (CDCInterfaceInfo->State.ControlLineStates.HostToDevice & CDC_CONTROL_LINE_OUT_DTR) != 0; }