Popularny manipulator sensoryczny na czujnikach AT42QT1011 ma kilka wad, dość trudno o regulację nacisku oraz jest to czujnik pojemnościowy i warunki środowiskowe mają dość spory wpływ na jego reakcję.
Postanowiłem wykorzystać czujnik nacisku - nie posiada ona wac czujnika pojemnościowego jak i zachowuje jego zalety - brak części ruchomych.
Wykorzystałem to tego belkę 0-100g (przynajmniej tak ją opisali chińczycy) - choć z pewnością zrobię testy także na belce 10N (1kg), którą już w SP można dostać za 30-parę zł.
Do okiełznania takiej belki niezbędny jest czuły przetwornik ADC z możliwością pomiaru dyferencjalnego. Użyty przeze mnie ADS1115 ma 2 kanały więc za jednym zamachem można obsłużyć dwie łopatki z jednego układu (Biblioteka ADS1115_lite). Odrobina kodu i manipulator gotowy ;-). Wygląda to obiecująco i na pewno przygotuję pod niego mechanikę (a w sumie to przerobię mój stary manipulator pojemnościowy) na początek spróbuję tymczasowego rozwiązania, które pozwoli na przetestowanie także innych belek (np. już wspomnianej).
// SP7Q - touch paddle with load cell
#include
#include
#include
ADS1115_lite ads1115;
//pin assing
int left = 12; // left paddle output
int right = 13; //right paddle output
int led1 = 37; //left paddle led pin if used
int led2 = 22; // righr paddle led if usedADS1115_lite ads1115;
int leftOn; //left paddle treshold
int leftOnAbs; //left paddle absolute value treshold
int leftOff; //left paddle off treshold
int rightOn; //right paddle treshold
int rightOnAbs; //right paddle absolute value treshold
int rightOff; //right paddle off treshold
int leftNow; //paddle pressed status
int rightNow; //paddle pressed status
int hysteresis = 7; //hysteresis
int16_t resultLeft; //tension read results
int16_t resultRight; //tension read results
int sensitivity; //sensitivity value
int sensitivityNow;
int configMode = 0;
int time1; //time for setup mode purpose
int time2;
int time3;
int time4;
int time5;
int ledy1 = 36;
int ledy2 = 35;
int ledy3 = 34;
int ledy4 = 33;
int ledy5 = 32;
int ledy6 = 31;
int ledy7 = 30;
int ledr1 = 23;
int ledr2 = 24;
int ledr3 = 25;
int ledr4 = 26;
int ledr5 = 27;
int ledr6 = 28;
int ledr7 = 29;
void setup() {
pinMode(left, OUTPUT); //I/Os define
pinMode(right, OUTPUT);
pinMode(led1, OUTPUT);
pinMode(led2, OUTPUT);
pinMode(ledy1, OUTPUT);
pinMode(ledy2, OUTPUT);
pinMode(ledy3, OUTPUT);
pinMode(ledy4, OUTPUT);
pinMode(ledy5, OUTPUT);
pinMode(ledy6, OUTPUT);
pinMode(ledy7, OUTPUT);
pinMode(ledr1, OUTPUT);
pinMode(ledr2, OUTPUT);
pinMode(ledr3, OUTPUT);
pinMode(ledr4, OUTPUT);
pinMode(ledr5, OUTPUT);
pinMode(ledr6, OUTPUT);
pinMode(ledr7, OUTPUT);
EEPROM.get(0,sensitivity); //get values from EEPROM
EEPROM.get(2,leftOn);
EEPROM.get(4,leftOff);
EEPROM.get(6,rightOn);
EEPROM.get(8,rightOff);
ads1115.setGain(ADS1115_REG_CONFIG_PGA_0_256V); //gain and sps define fos ADC
ads1115.setSampleRate(ADS1115_REG_CONFIG_DR_860SPS); //To have sample rate as fast as possible
/*serial only for testing purpose - comment if not needed
Serial.begin(250000); */
//read tension during startup to enter setup mode - both paddles should be squeezed
// Left Paddle read tension
ads1115.setMux(ADS1115_REG_CONFIG_MUX_DIFF_0_1);
ads1115.triggerConversion(); //Start a conversion.
resultLeft = ads1115.getConversion(); //This polls the ADS1115 and wait for conversion to finish, THEN returns the value]\
// Right Paddle read tension
ads1115.setMux(ADS1115_REG_CONFIG_MUX_DIFF_2_3);
ads1115.triggerConversion(); //Start a conversion.
resultRight = ads1115.getConversion(); //This polls the ADS1115 and wait for conversion to finish, THEN returns the value
if ((resultLeft > 20) && (resultRight > 20)){
configMode = 1;
time1 = millis();
}
}
void loop() {
//time3 = millis();
if (configMode){
config_loop();
}
else {
main_loop();
}
// time4 = millis();
// time5 = time3 - time4;
// Serial.println(time5);
}
void main_loop() {
// Left Paddle read tension
ads1115.setMux(ADS1115_REG_CONFIG_MUX_DIFF_0_1);
ads1115.triggerConversion(); //Start a conversion.
resultLeft = ads1115.getConversion(); //This polls the ADS1115 and wait for conversion to finish, THEN returns the value]
// Right Paddle read tension
ads1115.setMux(ADS1115_REG_CONFIG_MUX_DIFF_2_3);
ads1115.triggerConversion(); //Start a conversion.
resultRight = ads1115.getConversion(); //This polls the ADS1115 and wait for conversion to finish, THEN returns the value
//set outputs accordingly to tresholds
if (resultLeft > leftOn) {
digitalWrite(left, HIGH);
digitalWrite(led1, HIGH);
}
if (resultLeft < leftOff) {
digitalWrite(left, LOW);
digitalWrite(led1, LOW);
}
if (resultRight > rightOn) {
digitalWrite(right, HIGH);
digitalWrite(led2, HIGH);
}
if (resultRight < rightOff) {
digitalWrite(right, LOW);
digitalWrite(led2, LOW);
}
/*serial - only for testing purpose - comment if not needed
Serial.print("sensitivity");Serial.print(sensitivity);Serial.print("==");
Serial.print("leftOn");Serial.print(leftOn);Serial.print("==");
Serial.print("leftOff");Serial.print(leftOff);Serial.print("==");
Serial.print("rightOn");Serial.print(rightOn);Serial.print("==");
Serial.print("rightOff");Serial.print(rightOff);Serial.print("==");
Serial.print(resultLeft); Serial.print("<>");Serial.println(resultRight); */
//test leds
if (resultLeft > 10){
digitalWrite(ledy1, HIGH);
}
else {
digitalWrite(ledy1, LOW);
}
if (resultLeft > 30){
digitalWrite(ledy2, HIGH);
}
else {
digitalWrite(ledy2, LOW);
}
if (resultLeft > 50){
digitalWrite(ledy3, HIGH);
}
else {
digitalWrite(ledy3, LOW);
}
if (resultLeft > 70){
digitalWrite(ledy4, HIGH);
}
else {
digitalWrite(ledy4, LOW);
}
if (resultLeft > 90){
digitalWrite(ledy5, HIGH);
}
else {
digitalWrite(ledy5, LOW);
}
if (resultLeft > 110){
digitalWrite(ledy6, HIGH);
}
else {
digitalWrite(ledy6, LOW);
}
if (resultLeft > 130){
digitalWrite(ledy7, HIGH);
}
else {
digitalWrite(ledy7, LOW);
}
//test leds cd
if (resultRight > 10){
digitalWrite(ledr1, HIGH);
}
else {
digitalWrite(ledr1, LOW);
}
if (resultRight > 30){
digitalWrite(ledr2, HIGH);
}
else {
digitalWrite(ledr2, LOW);
}
if (resultRight > 50){
digitalWrite(ledr3, HIGH);
}
else {
digitalWrite(ledr3, LOW);
}
if (resultRight > 70){
digitalWrite(ledr4, HIGH);
}
else {
digitalWrite(ledr4, LOW);
}
if (resultRight > 90){
digitalWrite(ledr5, HIGH);
}
else {
digitalWrite(ledr5, LOW);
}
if (resultRight > 110){
digitalWrite(ledr6, HIGH);
}
else {
digitalWrite(ledr6, LOW);
}
if (resultRight > 130){
digitalWrite(ledr7, HIGH);
}
else {
digitalWrite(ledr7, LOW);
}
}
void config_loop(){
//config loop
ads1115.setMux(ADS1115_REG_CONFIG_MUX_DIFF_0_1);
ads1115.triggerConversion(); //Start a conversion.
resultLeft = ads1115.getConversion(); //This polls the ADS1115 and wait for conversion to finish, THEN returns the value]
ads1115.setMux(ADS1115_REG_CONFIG_MUX_DIFF_2_3);
ads1115.triggerConversion(); //Start a conversion.
resultRight = ads1115.getConversion(); //This polls the ADS1115 and wait for conversion to finish, THEN returns the value
time2 = millis(); //get time to leave config mode if nothing happen
leftOn = resultLeft + sensitivity; //measure free state and do the calibration
if (leftOn >= 0){
leftOff = leftOn - hysteresis;
}
else {
leftOnAbs = abs(leftOn);
leftOff = -hysteresis - leftOnAbs;
}
rightOn = resultRight + sensitivity;
if (rightOn >= 0){
rightOff = rightOn - hysteresis;
}
else {
rightOnAbs = abs(rightOn);
rightOff = -hysteresis - rightOnAbs;
}
//exit after 10s and write sensitivity/tresholds permanet
if ((time2-time1)>10000){
blink(sensitivity);
EEPROM.put(0,sensitivity); //store sensitivity permanent when leaving
EEPROM.put(2,leftOn);
EEPROM.put(4,leftOff);
EEPROM.put(6,rightOn);
EEPROM.put(8,rightOff);
configMode = 0;
}
//set paddle status
if ((resultLeft > 20) && (leftNow == 0)) {
leftNow = 1;
sensitivity = sensitivity + 10;
}
if (resultLeft < 10) {
leftNow = 0;
}
if ((resultRight > 20) && (rightNow == 0)) {
rightNow = 1;
sensitivity = sensitivity - 10;
}
if (resultRight < 10) {
rightNow = 0;
}
if (sensitivity != sensitivityNow)
switch (sensitivity){
case 0:
sensitivity = 10;
time1 = millis();
break;
case 10:
sensitivityNow = sensitivity;
blink(sensitivityNow);
time1 = millis();
break;
case 20:
sensitivityNow = sensitivity;
blink(sensitivityNow);
time1 = millis();
break;
case 30:
sensitivityNow = sensitivity;
blink(sensitivityNow);
time1 = millis();
break;
case 40:
sensitivityNow = sensitivity;
blink(sensitivityNow);
time1 = millis();
break;
case 50:
sensitivityNow = sensitivity;
blink(sensitivityNow);
time1 = millis();
break;
case 60:
sensitivityNow = sensitivity;
blink(sensitivityNow);
time1 = millis();
break;
case 70:
sensitivity = 60;
time1 = millis();
break;
default:
sensitivity = 10;
time1 = millis();
break;
}
/* Serial communication - only for diagnostic purpose
Serial.print("sensitivity");Serial.print(sensitivity);Serial.print("==");
Serial.print("leftOn");Serial.print(leftOn);Serial.print("==");
Serial.print("leftOff");Serial.print(leftOff);Serial.print("==");
Serial.print("rightOn");Serial.print(rightOn);Serial.print("==");
Serial.print("rightOff");Serial.print(rightOff);Serial.print("==");
Serial.print(resultLeft); Serial.print("<>");Serial.println(resultRight);*/
}
void blink(int amount){
for(int x = 0; x < amount; x = x+10){
digitalWrite(led1, HIGH);
delay(200);
digitalWrite(led1, LOW);
delay(200);
Serial.print(x);
}
}
Komentarze