sylvie-2024

Unnamed repository; edit this file 'description' to name the repository.
Log | Files | Refs | README | LICENSE

handMech_PCA9685.ino (3095B)

      1 #include <Wire.h>
      2 #include <Adafruit_PWMServoDriver.h>
      3 
      4 Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
      5 
      6 #define SERVOMIN  125 //  minimum pulse length count (out of 4096)
      7 #define SERVOMAX  600 // maximum pulse length count (out of 4096)
      8 
      9 uint8_t sg90_1 = 0;
     10 uint8_t sg90_2 = 1;
     11 uint8_t sg90_3 = 2;
     12 uint8_t sg90_4 = 3;
     13 uint8_t sg90_5 = 4;
     14 
     15 uint8_t MG996R_0 = 5;
     16 uint8_t MG996R_1 = 6;
     17 
     18 void setup() {
     19   pwm.begin();
     20   pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates
     21 
     22   pwm.setPWM(sg90_1, 0, angleToPulse(90));
     23   pwm.setPWM(sg90_2, 0, angleToPulse(90));
     24 
     25   pwm.setPWM(sg90_3, 0, angleToPulse(90));
     26   pwm.setPWM(sg90_4, 0, angleToPulse(90));
     27 
     28   pwm.setPWM(sg90_5, 0, angleToPulse(90));
     29 
     30   pwm.setPWM(MG996R_0, 0, angleToPulse(90));
     31   pwm.setPWM(MG996R_1, 0, angleToPulse(90));
     32 
     33   Serial.begin(9600);
     34   Serial.setTimeout(10); 
     35 
     36   // Start the I2C Bus as Master
     37   Wire.begin();   
     38 }
     39 
     40 int angleToPulse(int ang){
     41    int pulse = map(ang,0, 180, SERVOMIN,SERVOMAX);// map angle of 0 to 180 to Servo min and Servo max 
     42    return pulse;
     43 }
     44 
     45 long servoState = 0;
     46 
     47 void loop() {
     48   while (Serial.available() == 0) {}
     49   servoState = Serial.parseInt();
     50   
     51   Serial.print("Received command: ");
     52   Serial.println(servoState);    
     53 
     54   if(servoState > 0) { 
     55   
     56     if(servoState == 2){
     57       pwm.setPWM(sg90_1, 0, angleToPulse(180));
     58     }
     59     else if(servoState == 3){
     60      pwm.setPWM(sg90_1, 0, angleToPulse(0));           
     61     }  
     62     else if(servoState == 4){      
     63      pwm.setPWM(sg90_2, 0, angleToPulse(180));  
     64     } 
     65     else if (servoState == 5){
     66      pwm.setPWM(sg90_2, 0, angleToPulse(0));
     67     }    
     68     else if (servoState == 6){ 
     69      pwm.setPWM(sg90_3, 0, angleToPulse(180));             
     70     }
     71     else if (servoState == 7){      
     72      pwm.setPWM(sg90_3, 0, angleToPulse(0));               
     73     }
     74     else if (servoState == 8){
     75      pwm.setPWM(sg90_4, 0, angleToPulse(180));
     76     }
     77     else if (servoState == 9){
     78      pwm.setPWM(sg90_4, 0, angleToPulse(0));  
     79     }
     80     else if (servoState == 11){   
     81      pwm.setPWM(sg90_5, 0, angleToPulse(160));    
     82     }
     83     else if (servoState == 12){
     84      pwm.setPWM(sg90_5, 0, angleToPulse(0));  
     85     }
     86     else if (servoState == 13){
     87      pwm.setPWM(MG996R_0, 0, angleToPulse(180));  
     88     }    
     89     else if (servoState == 14){
     90      pwm.setPWM(MG996R_0, 0, angleToPulse(0));  
     91     }    
     92     else if (servoState == 15){
     93      pwm.setPWM(MG996R_1, 0, angleToPulse(90));  
     94     }    
     95     else if (servoState == 16){
     96      pwm.setPWM(MG996R_1, 0, angleToPulse(70));  
     97     }
     98     else if (servoState == 17) {
     99       Wire.beginTransmission(0x10);
    100       Wire.write(50);
    101       Wire.endTransmission();
    102     }
    103     else if (servoState == 18) {
    104       Wire.beginTransmission(0x10);
    105       Wire.write(51);
    106       Wire.endTransmission();  
    107     }
    108     else if (servoState == 19){
    109       Wire.beginTransmission(0x20);
    110       Wire.write(50);
    111       Wire.endTransmission();  
    112     }  
    113     else if (servoState == 20){
    114       Wire.beginTransmission(0x20);
    115       Wire.write(51);
    116       Wire.endTransmission();  
    117     } 
    118     Serial.begin(9600);
    119     Serial.setTimeout(10);  
    120   }
    121 }