bipedalRobot_SuperMaster.ino (6324B)
1 #include <Arduino.h> 2 #include <Wire.h> 3 4 #define I2C_SLAVE_ADDR_0 0x60 5 #define I2C_SLAVE_ADDR_1 0x61 6 #define I2C_SLAVE_ADDR_2 0x62 7 #define I2C_SLAVE_ADDR_3 0x61 8 #define I2C_SLAVE_ADDR_4 0x62 9 #define I2C_SLAVE_ADDR_5 0x61 10 #define I2C_SLAVE_ADDR_6 0x62 11 #define I2C_SLAVE_ADDR_7 0x61 12 #define I2C_SLAVE_ADDR_8 0x62 13 #define I2C_SLAVE_ADDR_9 0x61 14 #define I2C_SLAVE_ADDR_10 0x62 15 16 //const int sclPin = D1; 17 //const int sdaPin = D2; 18 19 const unsigned int MAX_MESSAGE_LENGTH = 125; 20 21 int currentSpeed = 10; 22 int currentAmps = 1000; 23 float currentAngle = 0; 24 25 float current_kp = 6.00; 26 float current_ki = 5.00; 27 float current_kd = 0.01; 28 29 bool containsSemicolon = false; 30 bool requestFlagOn = false; 31 byte requestedAddress; 32 33 char readWire[8]; 34 35 void setup() 36 { 37 Serial.begin(115200); 38 Serial.setTimeout(10); 39 40 Wire.setClock(400000); 41 Wire.begin(); 42 //Wire.begin(sdaPin, sclPin); 43 } 44 45 void sendMessage(char message[], int message_pos) { 46 //Add null character to string 47 char *messageTrimmed = &message[1]; 48 bool useLocal = false; 49 int msgStart = 0; 50 51 if (message[0] == '0') { 52 Wire.beginTransmission(I2C_SLAVE_ADDR_0); 53 useLocal = true; 54 requestedAddress = I2C_SLAVE_ADDR_0; 55 } else if (message[0] == '1') { 56 Wire.beginTransmission(I2C_SLAVE_ADDR_1); 57 requestedAddress = I2C_SLAVE_ADDR_1; 58 } else if (message[0] == '2') { 59 Wire.beginTransmission(I2C_SLAVE_ADDR_2); 60 requestedAddress = I2C_SLAVE_ADDR_2; 61 } else if (message[0] == '3') { 62 Wire.beginTransmission(I2C_SLAVE_ADDR_3); 63 requestedAddress = I2C_SLAVE_ADDR_3; 64 } else if (message[0] == '4') { 65 Wire.beginTransmission(I2C_SLAVE_ADDR_4); 66 requestedAddress = I2C_SLAVE_ADDR_4; 67 } else if (message[0] == '5') { 68 Wire.beginTransmission(I2C_SLAVE_ADDR_5); 69 requestedAddress = I2C_SLAVE_ADDR_5; 70 } else if (message[0] == '6') { 71 Wire.beginTransmission(I2C_SLAVE_ADDR_6); 72 requestedAddress = I2C_SLAVE_ADDR_6; 73 } else if (message[0] == '7') { 74 Wire.beginTransmission(I2C_SLAVE_ADDR_7); 75 requestedAddress = I2C_SLAVE_ADDR_7; 76 } else if (message[0] == '8') { 77 Wire.beginTransmission(I2C_SLAVE_ADDR_8); 78 requestedAddress = I2C_SLAVE_ADDR_8; 79 } else if (message[0] == '9') { 80 Wire.beginTransmission(I2C_SLAVE_ADDR_9); 81 requestedAddress = I2C_SLAVE_ADDR_9; 82 } else if (message[0] == 'a') { 83 Wire.beginTransmission(I2C_SLAVE_ADDR_10); 84 requestedAddress = I2C_SLAVE_ADDR_10; 85 } 86 87 if (useLocal == true){ 88 msgStart = 1; 89 } 90 91 for (msgStart; msgStart < message_pos; msgStart++){ 92 Wire.write(message[msgStart]); 93 if (message[msgStart] == '@') { requestFlagOn = true; } 94 } 95 96 Wire.endTransmission(); 97 98 //Print the message (or do other things) 99 if (messageTrimmed[0] == 'm') { 100 messageTrimmed[0] = '0'; 101 102 if (messageTrimmed[1] == '-') { 103 messageTrimmed[1] = '0'; 104 float newAngle = atof(messageTrimmed); 105 currentAngle = -newAngle; 106 } else { 107 currentAngle = atof(messageTrimmed); 108 } 109 Serial.print("New angle: "); 110 Serial.println(currentAngle); 111 } 112 else if (messageTrimmed[0] == 's') { 113 messageTrimmed[0] = '0'; 114 int newSpeed = atoi(messageTrimmed); 115 currentSpeed = newSpeed; 116 Serial.print("New speed: "); 117 Serial.println(currentSpeed); 118 } 119 else if (messageTrimmed[0] == 'c') { 120 messageTrimmed[0] = '0'; 121 if (atoi(messageTrimmed) <= 1200) { 122 int newAmps = atoi(messageTrimmed); 123 currentAmps = newAmps; 124 Serial.print("New current: "); 125 Serial.println(currentAmps); 126 } else { 127 Serial.println("Current too high!"); 128 } 129 } 130 131 // PID tuning on the fly 132 else if (messageTrimmed[0] == 'p') { 133 messageTrimmed[0] = '0'; 134 float new_kp = atof(messageTrimmed); 135 current_kp = new_kp; 136 Serial.print("New kP: "); 137 Serial.println(current_kp); 138 } 139 else if (messageTrimmed[0] == 'i') { 140 messageTrimmed[0] = '0'; 141 float new_ki = atof(messageTrimmed); 142 current_ki = new_ki; 143 Serial.print("New kI: "); 144 Serial.println(current_ki); 145 } 146 else if (messageTrimmed[0] == 'd') { 147 messageTrimmed[0] = '0'; 148 float new_kd = atof(messageTrimmed); 149 current_kd = new_kd; 150 Serial.print("New kD: "); 151 Serial.println(current_kd); 152 } 153 } 154 155 void loop() { 156 if (requestFlagOn == true){ 157 delay(30); 158 Wire.requestFrom(requestedAddress, 6, true); // request 8 bytes from selected slave device 159 160 int wireIndex = 0; 161 while(Wire.available()) // slave may send less than requested 162 { 163 readWire[wireIndex] = Wire.read(); // receive a byte as character 164 wireIndex++; 165 } 166 167 readWire[wireIndex] = '\0'; // assume it is a string, add zero-terminator 168 Serial.print("Requested value: "); 169 Serial.println(readWire); 170 requestFlagOn = false; 171 } else { 172 //Check to see if anything is available in the serial receive buffer 173 while (Serial.available() > 0) 174 { 175 //Create a place to hold the incoming message 176 static char message[MAX_MESSAGE_LENGTH]; 177 static unsigned int message_pos = 0; 178 179 //Read the next available byte in the serial receive buffer 180 char inByte = Serial.read(); 181 182 //Message coming in (check not terminating character) and guard for over message size 183 if ( inByte != '\n' && (message_pos < MAX_MESSAGE_LENGTH - 1) ) 184 { 185 //Add the incoming byte to our message 186 message[message_pos] = inByte; 187 if (message[message_pos] == ';'){ 188 containsSemicolon = true; 189 } 190 message_pos++; 191 } 192 //Full message received... 193 else 194 { 195 message[message_pos] = '\0'; 196 if (containsSemicolon == true){ 197 char *moveStrings[6]; // an array of pointers to the pieces of the above array after strtok() 198 char *ptr = NULL; 199 int moveIndex = 0; 200 201 ptr = strtok(message, ";"); // delimiter 202 while (ptr != NULL) { 203 moveStrings[moveIndex] = ptr; 204 moveIndex++; 205 ptr = strtok(NULL, ";"); 206 } 207 int i; 208 for (i = 0; i < moveIndex; i++) { 209 int thisLength = strlen(moveStrings[i]) + 1; 210 sendMessage(moveStrings[i], thisLength); 211 } 212 }else{ 213 sendMessage(message, message_pos); 214 } 215 //Reset for the next message 216 message_pos = 0; 217 containsSemicolon = false; 218 } 219 } 220 } 221 }