SHARE
TWEET

Untitled

a guest May 27th, 2019 88 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <msp430.h>
  2.  
  3. void MotorSetup();
  4. void SetLeftMotorSpeed();
  5. void SetRightMotorSpeed();
  6. void SetSpeeds(int lspeed,int rspeed);
  7. void IRSensorSetup();
  8. //void SonarSetup();
  9. //void ReadSonar();
  10. int readLine();
  11. void lineFollow();
  12. void SetBrakes();
  13.  
  14.  
  15. /**
  16.  * main.c
  17.  */
  18. //unsigned int up_counter;
  19. //unsigned int distance_cm;
  20.  
  21.  
  22. int val = 0;
  23. int sensorpanelVal = 0;
  24. int lastval = 0;
  25.  
  26.  
  27.  
  28. //PID Values
  29. int error = 0;
  30. int lasterror = 0;
  31.  
  32. #define BaseSpeed 160
  33. #define Kp 18
  34. #define Kd 30
  35.  
  36.  
  37.  
  38. int main(void)
  39. {
  40.     WDTCTL = WDTPW | WDTHOLD;   // stop watchdog timer
  41.     MotorSetup();
  42.     IRSensorSetup();
  43.     //SonarSetup();
  44.  
  45.     while(1){
  46.         lineFollow();
  47.         if(sensorpanelVal ==15){
  48.             SetBrakes();
  49.             while(1){
  50.  
  51.             }
  52.         }
  53.     }
  54.  
  55. }
  56.  
  57. #pragma vector=TIMER0_A0_VECTOR
  58. __interrupt void Timer_A (void)
  59. {
  60.     if (CCTL0 & CCI)            // Raising edge
  61.     {
  62.         //up_counter = CCR0;      // Copy counter to variable
  63.     }
  64.     else                        // Falling edge
  65.     {
  66.         // Formula: Distance in cm = (Time in uSec)/58
  67.         //distance_cm = (CCR0 - up_counter)/58;
  68.     }
  69.     TA0CTL &= ~TAIFG;           // Clear interrupt flag - handled
  70. }
  71.  
  72. void MotorSetup(){
  73.  
  74.     P2DIR |= BIT1+BIT5;//Pin 2.1 -> left motor speed Pin 2.5 -> Right Motor Speed
  75.     P2SEL |= BIT1+BIT5;
  76.  
  77.     P1DIR |= BIT3+BIT4; // Set Pins 1.3 and 1.4 as outputs to control left motor direction
  78.     P1DIR |= BIT5+BIT0; // Set Pins 1.5 and 1.6 as outputs to control right motor direction
  79.  
  80.     P1OUT &= ~(BIT1+BIT4+BIT5+BIT0); //set all pins to low
  81.  
  82.  
  83.  
  84.     /*** Timer1_A Set-Up ***/
  85.     TA1CCR0 |= 200 - 1;
  86.     TA1CCTL1 |= OUTMOD_7;
  87.     TA1CCTL2 |= OUTMOD_7;
  88.     TA1CCR1 |= 0;
  89.     TA1CCR2 |= 0;
  90.     TA1CTL |= TASSEL_2 + MC_1;
  91. }
  92.  
  93. void SetLeftMotorSpeed(int speed){
  94.     if (speed >0){
  95.         P1OUT &= ~BIT3; // Pin 1.3 Low
  96.         P1OUT |= BIT4; //Pin 1.4 High
  97.  
  98.         if(speed >199){
  99.             speed = 199;//prevent CCR2 from being negative
  100.         }
  101.         TA1CCR1 = speed;
  102.     }else if(speed <0){
  103.         P1OUT |= BIT3; //1.3 High
  104.         P1OUT &= ~BIT4; // Pin 1.4 Low
  105.         speed = -speed;
  106.  
  107.         if(speed >199){
  108.             speed = 199;//prevent CCR1 from being negative
  109.         }
  110.  
  111.         TA1CCR1 = speed;
  112.     }
  113. }
  114.  
  115. void SetRightMotorSpeed(int speed){
  116.     if (speed >0){
  117.         P1OUT &= ~BIT5;//Pin 1.5 Low
  118.         P1OUT |= BIT0; // Pin 1.3 Low
  119.  
  120.         if(speed >199){
  121.             speed = 199;//prevent CCR2 from being negative
  122.         }
  123.         TA1CCR2 = speed;
  124.     }else if(speed <0){
  125.         P1OUT |= BIT0; //1.5 High
  126.         P1OUT &= ~BIT6;//Pin 1.6 Low
  127.         speed = -speed;
  128.  
  129.         if(speed >199){
  130.             speed = 199; //prevent CCR2 from being negative
  131.         }
  132.         TA1CCR2 = speed;
  133.     }
  134. }
  135.  
  136. void SetSpeeds(int lspeed,int rspeed){
  137.     SetLeftMotorSpeed(lspeed);SetRightMotorSpeed(rspeed);
  138. }
  139.  
  140. void SetBrakes(){
  141.     P1OUT &= ~BIT5;//Pin 1.5 Low
  142.     P1OUT &= ~BIT0; // Pin 1.3 Low
  143.     P1OUT &= ~BIT3; // Pin 1.3 Low
  144.     P1OUT &= ~BIT4; //Pin 1.4 Low
  145.     TA1CCR1 = 199;
  146.     TA1CCR2 = 199;
  147. }
  148.  
  149. void IRSensorSetup(){
  150.     //Set IR sensor pins as inputs
  151.     P2DIR &= ~(BIT0+BIT2+BIT3);//+BIT4); //Set pins 2.0,2.2,2.3,2.4 as inputs
  152.     //P1DIR &= ~(BIT6); //1.7 as inputs
  153.  
  154.  
  155.  
  156. }
  157.  
  158.  
  159. int readLine()
  160. {
  161.     //from left to right. Sensor output is high when white space is detected. Since we're seeking a black line,inputs are inverted
  162.     int sensor1 = !(P2IN&BIT0);
  163.     int sensor2 = !(P2IN&BIT2);
  164.     int sensor3 = !(P2IN&BIT3);
  165.     //int sensor4 =!(P1IN&BIT6);
  166.     //int sensor5 =!(P2IN&BIT4);
  167.  
  168.  
  169.  
  170.     int sum = 0;
  171.  
  172.     sensorpanelVal = (sensor1 * 1)+(sensor2* 2)+(sensor3 * 3);//+(sensor4 *4)+(sensor5*5);
  173.     sum = (sensor1+sensor2+sensor3);//+sensor4+sensor5);
  174.  
  175.     if (sum ==0){
  176.         return lastval;
  177.     }else{
  178.  
  179.     lastval = sensorpanelVal/sum;
  180.     return lastval;
  181.     }
  182. }
  183.  
  184. void lineFollow(){
  185.     val = readLine();
  186.     error = 3-val;
  187.     int delta = error-lasterror;
  188.     int change = Kp*error + Kd*delta;
  189.     lasterror = error;
  190.  
  191.     int leftMotorPWM = BaseSpeed -change;
  192.     //constrain PWM
  193.     if(leftMotorPWM >199){
  194.         leftMotorPWM = 199;
  195.     }else if(leftMotorPWM <0){
  196.         leftMotorPWM = 0;
  197.     }
  198.     //constrain PWM
  199.     int rightMotorPWM = BaseSpeed + change;
  200.     if(rightMotorPWM >199){
  201.             rightMotorPWM = 199;
  202.     }else if(leftMotorPWM <0){
  203.             rightMotorPWM = 0;
  204.     }
  205.  
  206.  
  207.     SetSpeeds(leftMotorPWM,rightMotorPWM);
  208.  
  209. }
RAW Paste Data
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
 
Top