In the INO, I try: (line 76 in setup, and various places in loop)
tcaselect(0);
// #include "Wire.h"
// #define TCAADDR 0x70 //TCA8574 8 channel i2c mux
// #include <Adafruit_NAU7802.h> // i2c load cell adc
// Adafruit_NAU7802 PitchCell;
// Adafruit_NAU7802 RollCell;
// #include <Adafruit_PCF8574.h> // 8 channel digital IO
// #define PCFADDR 0x20
// Adafruit_PCF8574 pcf;
// #include <Adafruit_PWMServoDriver.h>
// Adafruit_PWMServoDriver board1 = Adafruit_PWMServoDriver(0x40);
// void tcaselect(uint8_t i) {
// if (i > 7) return;
// Wire.beginTransmission(TCAADDR);
// Wire.write(1 << i);
// Wire.endTransmission();
// }
#include <LoadCell.h>
float pitch, roll;
const int IN1 = 0; // i2c digitial IO
const int IN2 = 1; // i2c digitial IO
const int ENA = 0; // pwm channel 0
const int IN3 = 2; // i2c digitial IO
const int IN4 = 3; // i2c digitial IO
const int ENB = 1; // pwm channel 1
int slow = 512;
int pwm1 = slow;
int pwm2 = slow;
int fast = 4095;
float min1 = 2;
float minPitch = 15;
float minRoll = 30;
float norm = 150;
float normin = 100;
float normout = 100;
bool react = false;
bool lock = false;
void setup() {
while (!Serial);
delay(1000);
Wire.begin();
Serial.begin(115200);
board1.begin();
// if(!board1.begin()){
// Serial.println("Couldn't find servo driver board");
// while (1);
// }
board1.setPWMFreq(60); // Analog servos run at ~60 Hz updates
Serial.println();
delay(1000);
if (!pcf.begin(0x20, &Wire)) {
Serial.println("Couldn't find i2c mux board");
while (1);
}
for (uint8_t p=0; p<8; p++) {
pcf.pinMode(p, OUTPUT);
pcf.digitalWrite(p, LOW);
}
pcf.digitalWrite(0, LOW);
pcf.digitalWrite(1, LOW);
pcf.digitalWrite(2, LOW);
pcf.digitalWrite(3, LOW);
pcf.pinMode(IN1, OUTPUT);
pcf.pinMode(IN2, OUTPUT);
pcf.pinMode(ENA, OUTPUT);
pcf.pinMode(IN3, OUTPUT);
pcf.pinMode(IN4, OUTPUT);
pcf.pinMode(ENB, OUTPUT);
delay(100);
tcaselect(0);
delay(100);
Serial.print("----- PitchCell ");
if (! PitchCell.begin()) {
Serial.println("Failed to find NAU7802");
while (1) delay(10); // Don't proceed.
}
Serial.println("Found NAU7802");
PitchCell.setLDO(NAU7802_3V0);
PitchCell.setGain(NAU7802_GAIN_128);
PitchCell.setRate(NAU7802_RATE_80SPS);
// Take 10 readings to flush out readings
for (uint8_t i=0; i<10; i++) {
while (! PitchCell.available()) delay(10);
PitchCell.read();
}
while (! PitchCell.calibrate(NAU7802_CALMOD_INTERNAL)) {
Serial.println("Failed to calibrate internal offset, retrying!");
delay(1000);
}
Serial.println("Calibrated internal offset");
while (! PitchCell.calibrate(NAU7802_CALMOD_OFFSET)) {
Serial.println("Failed to calibrate system offset, retrying!");
delay(1000);
}
Serial.println("Calibrated system offset");
tcaselect(1);
Serial.print("----- RollCell ");
if (! RollCell.begin()) {
Serial.println("Failed to find NAU7802");
while (1) delay(10); // Don't proceed.
}
Serial.println("Found NAU7802");
RollCell.setLDO(NAU7802_3V0);
RollCell.setGain(NAU7802_GAIN_128);
RollCell.setRate(NAU7802_RATE_80SPS);
// Take 10 readings to flush out readings
for (uint8_t i=0; i<10; i++) {
while (! RollCell.available()) delay(10);
RollCell.read();
}
while (! RollCell.calibrate(NAU7802_CALMOD_INTERNAL)) {
Serial.println("Failed to calibrate internal offset, retrying!");
delay(1000);
}
Serial.println("Calibrated internal offset");
while (! RollCell.calibrate(NAU7802_CALMOD_OFFSET)) {
Serial.println("Failed to calibrate system offset, retrying!");
delay(1000);
}
Serial.println("Calibrated system offset");
Wire.begin();
}
void loop() {
int val;
if (Serial.available()){
char ch = Serial.read();
if(ch == 'f' || ch == 'F'){
react = false;
lock = false;
}
if(ch == 'r' || ch == 'R'){
react = true;
lock = false;
}
if(ch == 'l' || ch == 'L'){lock = true;}
if(ch == 'u' || ch == 'U'){lock = false;}
if(ch == 'c' || ch == 'C'){
tcaselect(0);
PitchCell.calibrate(NAU7802_CALMOD_OFFSET);
tcaselect(1);
RollCell.calibrate(NAU7802_CALMOD_OFFSET);
}
}
tcaselect(0);
while (! PitchCell.available()) {
delay(1);
}
val = PitchCell.read();
if (abs(val / 1000) > minPitch){
Serial.print("Pitch Read "); Serial.println(val/1000);
delay(100);
pitch = val/1000;
}
tcaselect(1);
while (! RollCell.available()) {
delay(1);
}
val = RollCell.read();
if (abs(val / 1000) > minRoll){
Serial.print("Roll Read "); Serial.println(val/1000);
delay(100);
roll = val/1000;
}
if(pitch < minPitch * -1){
if(react){
pcf.digitalWrite(IN1,HIGH);
pcf.digitalWrite(IN2,LOW);
}
else{
pcf.digitalWrite(IN1,LOW);
pcf.digitalWrite(IN2,HIGH);
}
pwm1 = slow;
}
if(roll < minRoll * -1){
if(react){
pcf.digitalWrite(IN4,HIGH);
pcf.digitalWrite(IN3,LOW);
}
else{
pcf.digitalWrite(IN4,LOW);
pcf.digitalWrite(IN3,HIGH);
}
pwm2 = slow;
}
if(pitch < normout * -1){
pwm1 = fast;
}
if(roll < normout * -1){
pwm2 = fast;
}
if(pitch > minPitch){
if(react){
pcf.digitalWrite(IN1,LOW);
pcf.digitalWrite(IN2,HIGH);
}
else{
pcf.digitalWrite(IN1,HIGH);
pcf.digitalWrite(IN2,LOW);
}
pwm1 = slow;
}
if(roll > minRoll){
if(react){
pcf.digitalWrite(IN4,LOW);
pcf.digitalWrite(IN3,HIGH);
}
else{
pcf.digitalWrite(IN4,HIGH);
pcf.digitalWrite(IN3,LOW);
}
pwm2 = slow;
}
if(pitch > normin){
pwm1 = fast;
}
if(roll > normin){
pwm2 = fast;
}
if(abs(pitch) < minPitch or lock){
pcf.digitalWrite(IN1,LOW);
pcf.digitalWrite(IN2,LOW);
pwm1 = 0;
}
if(abs(roll) < minRoll or lock){
pcf.digitalWrite(IN3,LOW);
pcf.digitalWrite(IN4,LOW);
pwm2 = 0;
}
board1.setPWM(ENA, 0, pwm1);
board1.setPWM(ENB, 0, pwm2);
}