API tools faq
paste
Advertisement
Guest User

Kandidat_3

a guest
May 26th, 2016
54
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 17.97 KB | None | 0 0
raw download clone embed print report
  1. #include <stdbool.h>
  2. #include <stdint.h>
  3. #include <stdlib.h>
  4. #include <math.h>
  5. #include "inc/hw_ints.h"
  6. #include "inc/hw_memmap.h"
  7. #include "inc/hw_types.h"
  8. #include "driverlib/adc.h"
  9. #include "driverlib/gpio.h"
  10. #include "driverlib/pin_map.h"
  11. #include "driverlib/sysctl.h"
  12. #include "driverlib/uart.h"
  13. #include "driverlib/interrupt.h"
  14. #include "utils/uartstdio.h"
  15. #include "driverlib/pwm.h"
  16. #include "driverlib/systick.c"
  17. #include "driverlib/systick.h"
  18.  
  19. const int pwm_period = 1500;
  20.  
  21. // Functions
  22. void sort(uint32_t sensor[60]);
  23.  
  24. void InitConsole(void)
  25. {
  26.     // Set the clock
  27.     SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ);
  28.  
  29.  
  30.     // UART
  31.  
  32.     // Enable GPIO port A which is used for UART0 pins.
  33.     SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
  34.     SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
  35.  
  36.     // Configure the pin muxing for UART0 functions on port A0 and A1.
  37.     // Configure the pin muxing for UART1 functions on port B0 and B1.
  38.     GPIOPinConfigure(GPIO_PA0_U0RX);
  39.     GPIOPinConfigure(GPIO_PA1_U0TX);
  40.     GPIOPinConfigure(GPIO_PB0_U1RX);
  41.     GPIOPinConfigure(GPIO_PB1_U1TX);
  42.  
  43.     // Enable UART0 and UART1
  44.     SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
  45.     SysCtlPeripheralEnable(SYSCTL_PERIPH_UART1);
  46.  
  47.     // Use the internal 16MHz oscillator as the UART clock source.
  48.     UARTClockSourceSet(UART0_BASE, UART_CLOCK_PIOSC);
  49.     UARTClockSourceSet(UART1_BASE, UART_CLOCK_PIOSC);
  50.  
  51.     // Select the alternate (UART) function for these pins.
  52.     GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
  53.     GPIOPinTypeUART(GPIO_PORTB_BASE, GPIO_PIN_0 | GPIO_PIN_1);
  54.  
  55.     // Initialize the UART for console I/O.
  56.     UARTStdioConfig(0, 9600, 16000000);
  57.     UARTStdioConfig(1, 9600, 16000000);
  58.  
  59.  
  60.     // PWM
  61.  
  62.     // Configure the PWM clock to match the system
  63.     SysCtlPWMClockSet(SYSCTL_PWMDIV_64);
  64.  
  65.     // Enable peripherals used by the program
  66.     SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);    //Enable control of GPIO B
  67.     SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);    //Enable control of GPIO E
  68.  
  69.     // Enable peripherals used by the program
  70.     SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM0);     //Enable control of module 0
  71.     SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM1);     //Enable control of module 1
  72.  
  73.     // Configure Pins as PWM
  74.     GPIOPinConfigure(GPIO_PB6_M0PWM0); //Configure PB6 as PWM0
  75.     GPIOPinConfigure(GPIO_PB7_M0PWM1); //Configure PB7 as PWM1 (inverted)
  76.     GPIOPinConfigure(GPIO_PE4_M1PWM2); //Configure PE4 as PWM2
  77.     GPIOPinConfigure(GPIO_PE5_M1PWM3); //Configure PE5 as PWM3 (inverted)
  78.     GPIOPinTypePWM(GPIO_PORTB_BASE, GPIO_PIN_7 | GPIO_PIN_6);
  79.     GPIOPinTypePWM(GPIO_PORTE_BASE, GPIO_PIN_4 | GPIO_PIN_5);
  80.  
  81.     // Configure PWM Options
  82.     PWMGenConfigure(PWM0_BASE, PWM_GEN_0, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_NO_SYNC);
  83.     PWMGenConfigure(PWM1_BASE, PWM_GEN_1, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_NO_SYNC);
  84.  
  85.     // Set the Period (expressed in clock ticks)
  86.     PWMGenPeriodSet(PWM0_BASE, PWM_GEN_0, pwm_period);
  87.     PWMGenPeriodSet(PWM1_BASE, PWM_GEN_1, pwm_period);
  88.  
  89.     // Set PWM duty
  90.     PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, pwm_period);
  91.     PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, pwm_period);
  92.     PWMPulseWidthSet(PWM1_BASE, PWM_OUT_2, pwm_period);
  93.     PWMPulseWidthSet(PWM1_BASE, PWM_OUT_3, pwm_period);
  94.  
  95.     // Turn on the Output pins
  96.     PWMOutputState(PWM0_BASE, PWM_OUT_0_BIT | PWM_OUT_1_BIT, true);
  97.     PWMOutputState(PWM1_BASE, PWM_OUT_2_BIT | PWM_OUT_3_BIT, true);
  98.  
  99.     // Enable the PWM generator
  100.     PWMGenEnable(PWM0_BASE, PWM_GEN_0);
  101.     PWMGenEnable(PWM1_BASE, PWM_GEN_1);
  102.  
  103.  
  104.     // Sensor
  105.  
  106.     // Enable ADC0 peripheral
  107.     SysCtlPeripheralEnable(SYSCTL_PERIPH_ADC0);
  108.  
  109.     // Sensors use GPIO ports D and E
  110.     SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
  111.     SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);
  112.  
  113.     // We want to use PD2, PD3, PE1 and PE2
  114.     GPIOPinTypeADC(GPIO_PORTD_BASE, GPIO_PIN_2); //CH5
  115.     GPIOPinTypeADC(GPIO_PORTD_BASE, GPIO_PIN_3); //CH4
  116.     GPIOPinTypeADC(GPIO_PORTE_BASE, GPIO_PIN_1); //CH2
  117.     GPIOPinTypeADC(GPIO_PORTE_BASE, GPIO_PIN_2); //CH1
  118.  
  119.     // Enable sample sequence 1
  120.     ADCSequenceConfigure(ADC0_BASE, 1, ADC_TRIGGER_PROCESSOR, 0);
  121.  
  122.     // Configure the steps on the channels
  123.     ADCSequenceStepConfigure(ADC0_BASE, 1, 0, ADC_CTL_CH1);
  124.     ADCSequenceStepConfigure(ADC0_BASE, 1, 1, ADC_CTL_CH2);
  125.     ADCSequenceStepConfigure(ADC0_BASE, 1, 2, ADC_CTL_CH4);
  126.     ADCSequenceStepConfigure(ADC0_BASE, 1, 3, ADC_CTL_CH5 | ADC_CTL_IE | ADC_CTL_END);
  127.  
  128.     // Enable sample sequence 1
  129.     ADCSequenceEnable(ADC0_BASE, 1);
  130.  
  131.     // Clear the interrupt status flag before sampling.
  132.     ADCIntClear(ADC0_BASE, 1);
  133. }
  134.  
  135. // Define order type
  136. struct DrivingOrders
  137. {
  138.     uint32_t number[2];
  139.     uint32_t direction;
  140.     uint32_t angle[3];
  141.     uint32_t distance[3];
  142. };
  143.  
  144. // Define order and create array of orders.
  145. typedef struct DrivingOrders order;
  146. order o[30];
  147.  
  148. // Function to delay
  149. void delayMS(int ms)
  150. {
  151.     SysCtlDelay((SysCtlClockGet() / (3 * 1000 )) * ms);
  152. }
  153.  
  154. // Function to receive order
  155. int GetOrders(order *o, int num)
  156. {
  157.     int i = 0;
  158.  
  159.     for(i = 0; i < num; i++)
  160.     {
  161.         // If not the first value received -> read white space
  162.         if (i > 0)
  163.         {
  164.             UARTCharGet(UART1_BASE);
  165.         }
  166.  
  167.         // Read driving order number
  168.         o[i].number[0] = UARTCharGet(UART1_BASE);
  169.         o[i].number[1] = UARTCharGet(UART1_BASE);
  170.  
  171.         // Read white space
  172.         UARTCharGet(UART1_BASE);
  173.  
  174.         // Read direction H/V/D/R
  175.         o[i].direction = UARTCharGet(UART1_BASE);
  176.  
  177.         // Read angle 000/045/090/135/180
  178.         o[i].angle[0] = UARTCharGet(UART1_BASE);
  179.         o[i].angle[1] = UARTCharGet(UART1_BASE);
  180.         o[i].angle[2] = UARTCharGet(UART1_BASE);
  181.  
  182.         // Read white space
  183.         UARTCharGet(UART1_BASE);
  184.  
  185.         // Read distance
  186.         o[i].distance[0] = UARTCharGet(UART1_BASE);
  187.         o[i].distance[1] = UARTCharGet(UART1_BASE);
  188.         o[i].distance[2] = UARTCharGet(UART1_BASE);
  189.     }
  190.  
  191.     // Return number of orders
  192.     return num;
  193. }
  194.  
  195. // Function to print order via USB
  196. void PrintOrder(order o[0], int n)
  197. {
  198.     int i = 0;
  199.     char temp  = ' ';
  200.     while(i < n)
  201.     {
  202.  
  203.         // Number
  204.         UARTCharPut(UART0_BASE, o[i].number[0]);
  205.         UARTCharPut(UART0_BASE, o[i].number[1]);
  206.         UARTCharPut(UART0_BASE, temp);
  207.  
  208.         // Direction
  209.         UARTCharPut(UART0_BASE, o[i].direction);
  210.  
  211.         // Angle
  212.         UARTCharPut(UART0_BASE, o[i].angle[0]);
  213.         UARTCharPut(UART0_BASE, o[i].angle[1]);
  214.         UARTCharPut(UART0_BASE, o[i].angle[2]);
  215.         UARTCharPut(UART0_BASE, temp);
  216.  
  217.         // Distance
  218.         UARTCharPut(UART0_BASE, o[i].distance[0]);
  219.         UARTCharPut(UART0_BASE, o[i].distance[1]);
  220.         UARTCharPut(UART0_BASE, o[i].distance[2]);
  221.         UARTCharPut(UART0_BASE, temp);
  222.  
  223.         i++;
  224.     }
  225. }
  226.  
  227. // Function to stop the vehicle
  228. void stop()
  229. {
  230.     uint32_t pwmNow = pwm_period;
  231.  
  232.     PWMOutputState(PWM0_BASE, (PWM_OUT_0_BIT|PWM_OUT_1_BIT), false);
  233.     PWMOutputState(PWM1_BASE, (PWM_OUT_2_BIT|PWM_OUT_3_BIT), false);
  234.  
  235.     PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, pwmNow);
  236.     PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, pwmNow);
  237.     PWMPulseWidthSet(PWM1_BASE, PWM_OUT_2, pwmNow);
  238.     PWMPulseWidthSet(PWM1_BASE, PWM_OUT_3, pwmNow);
  239. }
  240.  
  241. // Function to receive data from sensors
  242. uint32_t IRsensor(int loop)
  243. {
  244.     int i=0;
  245.     const uint16_t n = 10;
  246.     uint32_t sum;
  247.     uint32_t ADCvalue[4];
  248.     uint32_t sensor[10];
  249.     uint32_t dda;
  250.  
  251.     while(i < n - 1)
  252.     {
  253.         // Trigger the ADC conversion.
  254.         ADCProcessorTrigger(ADC0_BASE, 1);
  255.  
  256.         // Wait until the sample seq is completed
  257.         while(!ADCIntStatus(ADC0_BASE, 1, false))
  258.         {
  259.         }
  260.  
  261.         // Read ADC Value. Store from all sensors (ADCValue)
  262.         ADCSequenceDataGet(ADC0_BASE, 1, ADCvalue);
  263.  
  264.         // Store the value from the sensor in value
  265.         sensor[i] = ADCvalue[loop - 1];
  266.  
  267.         delayMS(3);
  268.         i++;
  269.     }
  270.  
  271.     // Reset the variables
  272.     sum=0;
  273.     i=4;
  274.  
  275.     while(i < 7)
  276.     {
  277.         sum+=sensor[i];
  278.         i++;
  279.     }
  280.  
  281.     sum = sum/3;
  282.  
  283.     // Use different algorithms depending on sensor
  284.  
  285.     // Sensor 1, 4 - 40 cm
  286.     if(loop == 1)
  287.     {
  288.         dda = 36.68*exp(-0.008286*sum)+41.56*exp(-0.001496*sum);
  289.  
  290.         if(dda > 30)
  291.             dda = 30;
  292.     }
  293.  
  294.     // Sensor 2, 4 - 40 cm
  295.     else if (loop == 2)
  296.     {
  297.         dda = 56.79*exp(-0.005785*sum)+40.74*exp(-0.001389*sum);
  298.  
  299.         if(dda > 30)
  300.             dda = 30;
  301.     }
  302.  
  303.     // Sensor 3, 8 - 80 cm
  304.     else if (loop == 3)
  305.     {
  306.         dda = 751.5*exp(-0.005364*sum)+53.89*exp(-0.0006045*sum);
  307.  
  308.         if(dda > 60)
  309.             dda = 60;
  310.     }
  311.  
  312.     // Sensor 4, 8 - 80 cm
  313.     else if (loop == 4)
  314.     {
  315.         dda = 479.2*exp(-0.004304*sum)+51.75*exp(-0.0005874*sum);
  316.  
  317.         if(dda > 60)
  318.             dda = 60;
  319.     }
  320.  
  321.    return dda;
  322. }
  323.  
  324. // Function to drive the vehicle
  325. int drive(uint32_t m, uint32_t dm)
  326. {
  327.     m = m - 48;
  328.     dm = dm - 48;
  329.     uint32_t pwmNow = pwm_period;
  330.     int i = 1;
  331.     uint32_t sensor[4];
  332.     int loop = 0;
  333.     int error = 0;
  334.  
  335.     loop = 2*(m*10 + dm);
  336.  
  337.     // Turn on the "forward" pins and off the "backward" pins
  338.     PWMOutputState(PWM0_BASE, PWM_OUT_1_BIT, false);
  339.     PWMOutputState(PWM1_BASE, PWM_OUT_2_BIT, false);
  340.     PWMOutputState(PWM0_BASE, PWM_OUT_0_BIT, true);
  341.     PWMOutputState(PWM1_BASE, PWM_OUT_3_BIT, true);
  342.  
  343.     // Set the pulse width
  344.     PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, pwmNow - 100);
  345.     PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, pwmNow);
  346.     PWMPulseWidthSet(PWM1_BASE, PWM_OUT_2, pwmNow);
  347.     PWMPulseWidthSet(PWM1_BASE, PWM_OUT_3, pwmNow);
  348.  
  349.     while(i <= loop)
  350.     {
  351.         // Collect data from sensors
  352.         sensor[0] = IRsensor(1); // Sensor 1 DH
  353.         sensor[1] = IRsensor(2); // Sensor 2 DV
  354.         sensor[2] = IRsensor(3); // Sensor 3 H
  355.         sensor[3] = IRsensor(4); // Sensor 4 V
  356.  
  357.         // If the sensor is 10 cm from an obstacle -> stop
  358.         if(sensor[0] <= 10) // DH
  359.         {
  360.             stop();
  361.             UARTprintf("V%d ", sensor[3]);
  362.             UARTprintf("DV%d ", sensor[1]);
  363.             UARTprintf("DH%d ", sensor[0]);
  364.             UARTprintf("H%d\n", sensor[2]);
  365.             UARTprintf("Unable to complete order, obstacle %d cm to the front-right.\n", sensor[0]);
  366.             i = loop+1;
  367.             error = 1;
  368.             break;
  369.         }
  370.  
  371.         // If the sensor is 10 cm from an obstacle -> stop
  372.         else if(sensor[1] <= 10) // DV
  373.         {
  374.             stop();
  375.             UARTprintf("V%d ", sensor[3]);
  376.             UARTprintf("DV%d ", sensor[1]);
  377.             UARTprintf("DH%d ", sensor[0]);
  378.             UARTprintf("H%d\n", sensor[2]);
  379.             UARTprintf("Unable to complete order, obstacle %d cm to the front-left.\n", sensor[1]);
  380.             i = loop+1;
  381.             error = 1;
  382.             break;
  383.         }
  384.  
  385.         // If stop-command -> stop
  386.         if(UARTCharGetNonBlocking(UART1_BASE) == 'S')
  387.         {
  388.             stop();
  389.             UARTprintf("V%d ", sensor[3]);
  390.             UARTprintf("DV%d ", sensor[1]);
  391.             UARTprintf("DH%d ", sensor[0]);
  392.             UARTprintf("H%d\n", sensor[2]);
  393.             i = loop+1;
  394.             error = 1;
  395.             break;
  396.         }
  397.  
  398.  
  399.         // Display the data
  400.         UARTprintf("V%d ", sensor[3]);
  401.         UARTprintf("DV%d ", sensor[1]);
  402.         UARTprintf("DH%d ", sensor[0]);
  403.         UARTprintf("H%d\n", sensor[2]);
  404.  
  405.         delayMS(25);
  406.  
  407.         i++;
  408.     }
  409.  
  410.  
  411.     stop();
  412.  
  413.     delayMS(1000); // Delay 1 second
  414.  
  415.     return error;
  416. }
  417.  
  418. int reverse(uint32_t m, uint32_t dm)
  419. {
  420.     m = m - 48;
  421.     dm = dm - 48;
  422.     uint32_t pwmNow = pwm_period;
  423.     int i = 1;
  424.     uint32_t sensor[4];
  425.     int loop = 0;
  426.     int error = 0;
  427.  
  428.     loop = 2*(m*10 + dm);
  429.  
  430.     // Turn on the "backward" pins and off the "forward" pins
  431.     PWMOutputState(PWM1_BASE, PWM_OUT_3_BIT, false);
  432.     PWMOutputState(PWM0_BASE, PWM_OUT_0_BIT, false);
  433.     PWMOutputState(PWM0_BASE, PWM_OUT_1_BIT, true);
  434.     PWMOutputState(PWM1_BASE, PWM_OUT_2_BIT, true);
  435.  
  436.     // Set the pulse width
  437.     PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, pwmNow);
  438.     PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, pwmNow - 47);
  439.     PWMPulseWidthSet(PWM1_BASE, PWM_OUT_2, pwmNow);
  440.     PWMPulseWidthSet(PWM1_BASE, PWM_OUT_3, pwmNow);
  441.  
  442.     while(i <= loop)
  443.     {
  444.         // Collect sensor data
  445.         sensor[0] = IRsensor(1); // Sensor 1 DH
  446.         sensor[1] = IRsensor(2); // Sensor 2 DV
  447.         sensor[2] = IRsensor(3); // Sensor 3 H
  448.         sensor[3] = IRsensor(4); // Sensor 4 V
  449.  
  450.         // If the sensor is 10 cm from an obstacle -> stop
  451.         if(sensor[0] <= 10) // DH
  452.         {
  453.             stop();
  454.             UARTprintf("V%d ", sensor[3]);
  455.             UARTprintf("DV%d ", sensor[1]);
  456.             UARTprintf("DH%d ", sensor[0]);
  457.             UARTprintf("H%d\n", sensor[2]);
  458.             UARTprintf("Unable to complete order, obstacle %d cm to the front-right.\n", sensor[0]);
  459.             i = loop+1;
  460.             error = 1;
  461.             break;
  462.         }
  463.  
  464.         // If the sensor is 10 cm from an obstacle -> stop
  465.         else if(sensor[1] <= 10) // DV
  466.         {
  467.             stop();
  468.             UARTprintf("V%d ", sensor[3]);
  469.             UARTprintf("DV%d ", sensor[1]);
  470.             UARTprintf("DH%d ", sensor[0]);
  471.             UARTprintf("H%d\n", sensor[2]);
  472.             UARTprintf("Unable to complete order, obstacle %d cm to the front-left.\n", sensor[1]);
  473.             i = loop+1;
  474.             error = 1;
  475.             break;
  476.         }
  477.  
  478.         // If stop-command -> stop
  479.         if(UARTCharGetNonBlocking(UART1_BASE) == 'S') // Köra fram tills vi tar emot ett S (stop)
  480.         {
  481.             stop();
  482.             UARTprintf("V%d ", sensor[3]);
  483.             UARTprintf("DV%d ", sensor[1]);
  484.             UARTprintf("DH%d ", sensor[0]);
  485.             UARTprintf("H%d\n", sensor[2]);
  486.             i = loop+1;
  487.             error = 1;
  488.             break;
  489.         }
  490.  
  491.         // Display the data
  492.         UARTprintf("V%d ", sensor[3]);
  493.         UARTprintf("DV%d ", sensor[1]);
  494.         UARTprintf("DH%d ", sensor[0]);
  495.         UARTprintf("H%d\n", sensor[2]);
  496.  
  497.         delayMS(24);
  498.  
  499.         i++;
  500.  
  501.     }
  502.  
  503.     stop();
  504.  
  505.     delayMS(1000); // Delay 1 second
  506.  
  507.     return error;
  508. }
  509.  
  510. // Function to turn left
  511. void turn_left(int n)
  512. {
  513.     n = n - 48;
  514.     uint32_t pwmNow = pwm_period - 500;
  515.  
  516.     // Turn off the back-pin for the right side and front-pin for the left side
  517.     PWMOutputState(PWM1_BASE, PWM_OUT_2_BIT, false);
  518.     PWMOutputState(PWM0_BASE, PWM_OUT_0_BIT, false);
  519.  
  520.     // Turn on the back-pin for the left side and front-pin for the right side
  521.     PWMOutputState(PWM1_BASE, PWM_OUT_3_BIT, true);
  522.     PWMOutputState(PWM0_BASE, PWM_OUT_1_BIT, true);
  523.  
  524.     // Set the pulse width
  525.     PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, pwmNow);
  526.     PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, pwmNow - 47);
  527.     PWMPulseWidthSet(PWM1_BASE, PWM_OUT_2, pwmNow);
  528.     PWMPulseWidthSet(PWM1_BASE, PWM_OUT_3, pwmNow);
  529.  
  530.     // If 45 degrees
  531.     if(n == 4)
  532.         delayMS(575);
  533.  
  534.     // If 90 degrees
  535.     else if (n == 9)
  536.         delayMS(1115);
  537.  
  538.     // If 135 degrees
  539.     else if (n == 3)
  540.         delayMS(1650);
  541.  
  542.     // If 180 degrees
  543.     else if (n == 8)
  544.         delayMS(2170);
  545.  
  546.     stop();
  547.  
  548.     delayMS(250);
  549. }
  550.  
  551. // Function to turn right
  552. void turn_right(int n)
  553. {
  554.     n = n - 48;
  555.     uint32_t pwmNow = pwm_period - 500;
  556.  
  557.     // Turn on the front-pin for the right side and the back-pin for the left side
  558.     PWMOutputState(PWM1_BASE, PWM_OUT_3_BIT, false);
  559.     PWMOutputState(PWM0_BASE, PWM_OUT_1_BIT, false);
  560.  
  561.     // Turn on the front-pin for the left side and the back-pin for the right side
  562.     PWMOutputState(PWM1_BASE, PWM_OUT_2_BIT, true);
  563.     PWMOutputState(PWM0_BASE, PWM_OUT_0_BIT, true);
  564.  
  565.     // Set the pulse width
  566.     PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, pwmNow - 100);
  567.     PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, pwmNow);
  568.     PWMPulseWidthSet(PWM1_BASE, PWM_OUT_2, pwmNow);
  569.     PWMPulseWidthSet(PWM1_BASE, PWM_OUT_3, pwmNow);
  570.  
  571.     // If 45 degrees
  572.     if(n == 4)
  573.         delayMS(590);
  574.  
  575.     // If 90 degrees
  576.     else if (n == 9)
  577.         delayMS(1145);
  578.  
  579.     // If 135 degrees
  580.     else if (n == 3)
  581.         delayMS(1655);
  582.  
  583.     // If 180 degrees
  584.     else if (n == 8)
  585.         delayMS(2200);
  586.  
  587.     stop();
  588.  
  589.     delayMS(250);
  590. }
  591.  
  592. int main(void)
  593. {
  594.     // Variables
  595.     int command;
  596.     int orders = 0;
  597.     int orderT = 0, orderE;
  598.     int i = 0;
  599.     int error = 0;
  600.     uint32_t sensor1 = 0, sensor2 = 0, sensor3 = 0, sensor4 = 0;
  601.  
  602.     // Initialize
  603.     InitConsole();
  604.  
  605.     // Create different states for the state machine
  606.     enum states
  607.     {
  608.         wait,
  609.         mapping,
  610.         transport
  611.     };
  612.  
  613.     // Set default state to wait
  614.     enum states state = wait;
  615.  
  616.     while(1)
  617.     {
  618.         switch(state)
  619.         {
  620.         case wait:
  621.             stop();
  622.  
  623.             UARTprintf("Which state? \n");
  624.  
  625.             command = UARTCharGet(UART1_BASE);
  626.  
  627.             // If 'K'
  628.             if(command == 75)
  629.             {
  630.                 state = mapping;
  631.             }
  632.  
  633.             // If 'T'
  634.             else if (command == 84)
  635.             {
  636.                 state = transport;
  637.             }
  638.  
  639.             else
  640.             {
  641.                 UARTprintf("This state does not exist... \n");
  642.             }
  643.             break;
  644.  
  645.         case mapping:
  646.             // Collect data from sensors
  647.             sensor1 = IRsensor(1);
  648.             sensor2 = IRsensor(2);
  649.             sensor3 = IRsensor(3);
  650.             sensor4 = IRsensor(4);
  651.  
  652.             // Display the data
  653.             UARTprintf("V%d ", sensor4);
  654.             UARTprintf("DV%d ", sensor2);
  655.             UARTprintf("DH%d ", sensor1);
  656.             UARTprintf("H%d\n", sensor3);
  657.  
  658.             while(1) // Infinity loop until stop-command
  659.             {
  660.                 GetOrders(o, 1);
  661.                 PrintOrder(&o[0], 1);
  662.  
  663.                 // Perform the order
  664.                 if(o[0].direction == 'D')
  665.                 {
  666.                     drive(o[0].distance[0], o[0].distance[1]);
  667.                 }
  668.  
  669.                 else if(o[0].direction == 'R')
  670.                 {
  671.                     reverse(o[0].distance[0], o[0].distance[1]);
  672.                 }
  673.  
  674.                 else if(o[0].direction == 'V')
  675.                 {
  676.                     turn_left(o[0].angle[1]);
  677.                     drive(o[0].distance[0], o[0].distance[1]);
  678.                 }
  679.  
  680.                 else if(o[0].direction == 'H')
  681.                 {
  682.                     turn_right(o[0].angle[1]);
  683.                     drive(o[0].distance[0], o[0].distance[1]);
  684.                 }
  685.  
  686.                 else if(o[0].number[0] == 'S')
  687.                 {
  688.                     break;
  689.  
  690.                 }
  691.  
  692.                 stop();
  693.             }
  694.  
  695.             state = wait;
  696.             break;
  697.  
  698.         case transport:
  699.             UARTprintf("How many transport orders? \n");
  700.             orderT = UARTCharGet(UART1_BASE);
  701.             orderE = UARTCharGet(UART1_BASE);
  702.  
  703.             orders = (orderT-48)*10 + (orderE-48);
  704.  
  705.             command = GetOrders(o, orders);
  706.  
  707.             // Collect data from sensors
  708.             sensor1 = IRsensor(1);
  709.             sensor2 = IRsensor(2);
  710.             sensor3 = IRsensor(3);
  711.             sensor4 = IRsensor(4);
  712.  
  713.             // Display the data
  714.             UARTprintf("V%d ", sensor4);
  715.             UARTprintf("DV%d ", sensor2);
  716.             UARTprintf("DH%d ", sensor1);
  717.             UARTprintf("H%d\n", sensor3);
  718.  
  719.             for(i = 0; i < command; i++)
  720.             {
  721.                 // Perform the driving order
  722.  
  723.                 if(o[i].direction == 'D')
  724.                 {
  725.                     error = drive(o[i].distance[0], o[i].distance[1]);
  726.                 }
  727.  
  728.                 else if(o[i].direction == 'R')
  729.                 {
  730.                     error = reverse(o[i].distance[0], o[i].distance[1]);
  731.                 }
  732.  
  733.                 else if(o[i].direction == 'V')
  734.                 {
  735.                     turn_left(o[i].angle[1]);
  736.                      error = drive(o[i].distance[0], o[i].distance[1]);
  737.                 }
  738.  
  739.                 else if(o[i].direction == 'H')
  740.                 {
  741.                     turn_right(o[i].angle[1]);
  742.                     error = drive(o[i].distance[0], o[i].distance[1]);
  743.                 }
  744.  
  745.                 if(error == 1)
  746.                 {
  747.                     i = command;
  748.                     break;
  749.                 }
  750.  
  751.                 stop();
  752.             }
  753.  
  754.             if(error == 1)
  755.                 state = transport;
  756.  
  757.             else
  758.                 state = wait;
  759.  
  760.             break;
  761.         }
  762.  
  763.         SysCtlDelay(SysCtlClockGet() / 12);
  764.     }
  765.  
  766. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!