Changeset 1310f0c in pyramid


Ignore:
Timestamp:
11/12/13 14:15:51 (6 years ago)
Author:
Steve Castellotti <sc@…>
Branches:
master
Children:
0bad654
Parents:
3345535
Message:
  • test hop added
File:
1 edited

Legend:

Unmodified
Added
Removed
  • firmware/Puzzlebox_Pyramid/Puzzlebox_Pyramid.ino

    r3345535 r1310f0c  
    2828Original Author: Hao Zhang <hz@puzzlebox.info> 
    2929 
    30 Last Modified 2013-10-31 
     30Last Modified 2013-11-12 
    3131by Steve Castellotti <sc@puzzlebox.info> 
    3232 
     
    6262#define DEFAULT_YAW 78 
    6363#define DEFAULT_PITCH 31 
     64#define HOP_THROTTLE_MAX 64 
     65#define HOP_STEP 4 
    6466char _throttle=DEFAULT_THROTTLE; //32~127, default 0 
    6567char _yaw=DEFAULT_YAW; //16~127, default 78 
     
    11011103        int p3; 
    11021104         
     1105        int hopThrottle = DEFAULT_THROTTLE; 
     1106        int hopYaw = DEFAULT_YAW; 
     1107        int hopPitch = DEFAULT_PITCH; 
     1108         
    11031109        while (Serial.available() == 0); 
    11041110        direction = Serial.read(); 
     
    11071113        Serial.println(_command); 
    11081114         
     1115         
     1116        // Set Color Pattern 
     1117         
    11091118        setColorWheel(0, 0, 0); // black 
    11101119         
    11111120        if (direction == 'f') { 
    11121121                #if DEBUG_OUTPUT 
    1113                         Serial.print("Hop: Forward"); 
     1122                        Serial.println("Hop: Forward"); 
    11141123                #endif 
    11151124                p1 = 10; 
     
    11191128        else if (direction == 'r') { 
    11201129                #if DEBUG_OUTPUT 
    1121                         Serial.print("Hop: Right"); 
     1130                        Serial.println("Hop: Right"); 
    11221131                #endif 
    11231132                p1 = 1; 
     
    11271136        else if (direction == 'b') { 
    11281137                #if DEBUG_OUTPUT 
    1129                         Serial.print("Hop: Backwards"); 
     1138                        Serial.println("Hop: Backwards"); 
    11301139                #endif 
    11311140                p1 = 4; 
     
    11351144        else if (direction == 'l') { 
    11361145                #if DEBUG_OUTPUT 
    1137                         Serial.print("Hop: Left"); 
     1146                        Serial.println("Hop: Left"); 
    11381147                #endif 
    11391148                p1 = 7; 
     
    11601169         
    11611170         
     1171         
     1172        // Begin Hop 
     1173                #if DEBUG_OUTPUT 
     1174                        Serial.println("Hop: (Ascending)"); 
     1175                #endif 
     1176        while (hopThrottle < 64) { 
     1177                hopThrottle = hopThrottle + 5; 
     1178                if (hopThrottle > 64) 
     1179                        hopThrottle = 64; 
     1180                 
     1181                #if DEBUG_OUTPUT 
     1182                        Serial.print("Sending: ("); 
     1183                        Serial.print(hopThrottle); 
     1184                        Serial.print(", "); 
     1185                        Serial.print(hopYaw); 
     1186                        Serial.print(", "); 
     1187                        Serial.print(hopPitch); 
     1188                        Serial.println(")"); 
     1189                #endif 
     1190                 
     1191                sendCode(formCode(hopThrottle, hopYaw, hopPitch)); 
     1192                delay(80); //cycle(); 
     1193        } 
     1194         
     1195        // Descend 
     1196                #if DEBUG_OUTPUT 
     1197                        Serial.println("Hop: (Descending)"); 
     1198                #endif 
     1199        while (hopThrottle > DEFAULT_THROTTLE) { 
     1200                hopThrottle = hopThrottle - 5; 
     1201                if (hopThrottle < DEFAULT_THROTTLE) 
     1202                        hopThrottle = DEFAULT_THROTTLE; 
     1203                 
     1204                #if DEBUG_OUTPUT 
     1205                        Serial.print("Sending: ("); 
     1206                        Serial.print(hopThrottle); 
     1207                        Serial.print(", "); 
     1208                        Serial.print(hopYaw); 
     1209                        Serial.print(", "); 
     1210                        Serial.print(hopPitch); 
     1211                        Serial.println(")"); 
     1212                #endif 
     1213 
     1214                 
     1215                sendCode(formCode(hopThrottle, hopYaw, hopPitch)); 
     1216                delay(80); //cycle(); 
     1217        } 
     1218         
     1219         
     1220        sendCode(formCode(DEFAULT_PITCH, DEFAULT_YAW, DEFAULT_PITCH)); 
     1221         
     1222         
    11621223} // setColor() 
     1224 
     1225 
     1226// ################################################################ 
     1227 
     1228int calculateMeter(int value) { 
     1229 
     1230        int result = 0; 
     1231 
     1232        if (value == 0) 
     1233                result = 0; 
     1234        else if (value <= 20) 
     1235                result = 1; 
     1236        else if (value <= 40) 
     1237                result = 2; 
     1238        else if (value <= 60) 
     1239                result = 3; 
     1240        else if (value <= 80) 
     1241                result = 4; 
     1242        else if (value <= 100) 
     1243                result = 5; 
     1244 
     1245        return(result); 
     1246 
     1247} // calculateMeter 
     1248 
     1249 
     1250// ################################################################ 
     1251 
     1252void updateFrameADK() { 
     1253 
     1254        #if DEBUG_OUTPUT 
     1255                Serial.print("eegSignal: "); 
     1256                Serial.print(eegSignal, DEC); 
     1257                Serial.print(" | eegAttention: "); 
     1258                Serial.print(eegAttention, DEC); 
     1259                Serial.print(" | eegMeditation: "); 
     1260                Serial.print(eegMeditation, DEC); 
     1261                Serial.print(" | eegPower: "); 
     1262                Serial.println(eegPower, DEC); 
     1263                Serial.println(); 
     1264                 
     1265                Serial.print("Thottle: "); 
     1266                Serial.print(_throttle, DEC); 
     1267                Serial.print(" | Yaw: "); 
     1268                Serial.print(_yaw, DEC); 
     1269                Serial.print(" | Pitch: "); 
     1270                Serial.println(_pitch, DEC); 
     1271        #endif 
     1272         
     1273//      setColorWheel(0, 0, 0); // black 
     1274         
     1275         
     1276        // if the signal is good enough, light 6:00 LED in green. 
     1277        if (eegSignal == 100) { 
     1278                RGB_Panel[5][0]=0; 
     1279                RGB_Panel[5][1]=255; 
     1280                RGB_Panel[5][2]=0; 
     1281        } else { 
     1282                RGB_Panel[5][0]=0; 
     1283                RGB_Panel[5][1]=63; 
     1284                RGB_Panel[5][2]=0; 
     1285                // The following two lines can optionally be used 
     1286                // to set all lights to red to indicate ADK mode 
     1287                // when the EEG signal quality is insufficient for processing 
     1288//              setColorWheel(255, 0, 0); // red 
     1289//              return; 
     1290        } 
     1291         
     1292         
     1293        int attention = calculateMeter(eegAttention); 
     1294        int meditation = calculateMeter(eegMeditation); 
     1295         
     1296         
     1297                //light up & dim red LED according to attention level 
     1298        for(int i=6; i<6+attention; i++) { 
     1299                RGB_Panel[i][0]=255; 
     1300                RGB_Panel[i][1]=0; 
     1301                RGB_Panel[i][2]=0; 
     1302        } 
     1303        for(int i=6+attention; i<11; i++) { 
     1304                RGB_Panel[i][0]=0; 
     1305                RGB_Panel[i][1]=0; 
     1306                RGB_Panel[i][2]=0; 
     1307        } 
     1308 
     1309        //light up & dim blue LED according to meditation level 
     1310        for(int i=4; i>4-meditation; i--) { 
     1311                RGB_Panel[i][0]=0; 
     1312                RGB_Panel[i][1]=0; 
     1313                RGB_Panel[i][2]=255; 
     1314        } 
     1315        for(int i=4-meditation; i>-1; i--) { 
     1316                RGB_Panel[i][0]=0; 
     1317                RGB_Panel[i][1]=0; 
     1318                RGB_Panel[i][2]=0; 
     1319        } 
     1320 
     1321 
     1322        if (eegPower > 0) { 
     1323                RGB_Panel[11][0]=255; 
     1324                RGB_Panel[11][1]=255; 
     1325                RGB_Panel[11][2]=255; 
     1326//              _throttle=_throttle_hover; 
     1327//              Serial.println("Throttle On"); 
     1328        } else { 
     1329                RGB_Panel[11][0]=0; 
     1330                RGB_Panel[11][1]=0; 
     1331                RGB_Panel[11][2]=0; 
     1332//              _throttle=0; 
     1333//              Serial.println("Throttle Off"); 
     1334        } // eegPower 
     1335         
     1336         
     1337        sendFrame(0); 
     1338 
     1339 
     1340} // updateFrameADK() 
     1341 
     1342 
     1343// ################################################################ 
     1344 
     1345void parseADK() { 
     1346         
     1347        // Android ADK 
     1348         
     1349        if (acc.isConnected()) { 
     1350                 
     1351                int len = acc.read(msg, sizeof(msg), 1); 
     1352                 
     1353                if (! modeADK) { 
     1354                        modeADK = true; 
     1355                        modeBluetooth = false; 
     1356                        modeSerial = false; 
     1357                        Serial1.end(); 
     1358                        Serial.println("INFO: parseADK connection detected, setting modeADK"); 
     1359                        setColorWheel(255, 0, 0); // red 
     1360                } 
     1361                 
     1362                // Action taken by Arduino is tied to the message it receives from Android 
     1363                 
     1364                if (len > 0) { 
     1365                         
     1366//                      Serial.println("INFO: ADK message received"); 
     1367                         
     1368                        if (msg[0] == 0x1) { 
     1369//                              Serial.println("0x1"); 
     1370                                eegSignal = (int)msg[1]; 
     1371//                              Serial.println(eegSignal); 
     1372                        } 
     1373                         
     1374                        else if(msg[0] == 0x2) { 
     1375                                eegAttention = (int)msg[1]; 
     1376                        } 
     1377                         
     1378                        else if (msg[0] == 0x3) { 
     1379                                eegMeditation = (int)msg[1]; 
     1380                        } 
     1381                         
     1382                        else if (msg[0] == 0x4) { 
     1383                                eegPower = (int)msg[1]; 
     1384                        } 
     1385                         
     1386                        else if(msg[0] == 0x5) { 
     1387                                _throttle = (int)msg[1]; 
     1388                        } 
     1389                         
     1390                        else if (msg[0] == 0x6) { 
     1391                                _yaw = (int)msg[1]; 
     1392                        } 
     1393                         
     1394                        else if (msg[0] == 0x7) { 
     1395                                _pitch = (int)msg[1]; 
     1396                        } 
     1397                         
     1398                        else if (msg[0] == 0x8) { 
     1399                                if (msg[1] == 0x1) 
     1400                                        _channel = 'A'; 
     1401                                else if (msg[1] == 0x2) 
     1402                                        _channel = 'B'; 
     1403                                else if (msg[1] == 0x3) 
     1404                                        _channel = 'C'; 
     1405                        } 
     1406                         
     1407                         
     1408                        // Orbit Hover 
     1409                        if (msg[0] == 0x9) { 
     1410                                 
     1411                                if (msg[1] == 0x1) { 
     1412                                        setColorWheel(255,255,255); // white 
     1413                                        _throttle = _throttle_hover; 
     1414                                        _yaw = DEFAULT_YAW; 
     1415                                        _pitch = DEFAULT_PITCH; 
     1416                                } 
     1417                                else if (msg[1] == 0x0) { 
     1418                                        setColorWheel(255,0,0); // red 
     1419                                        _throttle = DEFAULT_THROTTLE; 
     1420                                        _yaw = DEFAULT_YAW; 
     1421                                        _pitch = DEFAULT_PITCH; 
     1422                                } 
     1423                                 
     1424                        } else { 
     1425                                // Update the color wheel with all values if not forcing hover mode 
     1426                                updateFrameADK(); 
     1427                        } // Hover 
     1428                         
     1429                 
     1430                } // len 
     1431                 
     1432                sendCode(formCode(_throttle,_yaw,_pitch)); 
     1433                delay(80); //cycle(); 
     1434                 
     1435                 
     1436        } // if acc.isConnected() 
     1437 
     1438 
     1439} // parseADK 
    11631440 
    11641441 
     
    12151492 
    12161493 
    1217 // ################################################################ 
    1218  
    1219 int calculateMeter(int value) { 
    1220  
    1221         int result = 0; 
    1222  
    1223         if (value == 0) 
    1224                 result = 0; 
    1225         else if (value <= 20) 
    1226                 result = 1; 
    1227         else if (value <= 40) 
    1228                 result = 2; 
    1229         else if (value <= 60) 
    1230                 result = 3; 
    1231         else if (value <= 80) 
    1232                 result = 4; 
    1233         else if (value <= 100) 
    1234                 result = 5; 
    1235  
    1236         return(result); 
    1237  
    1238 } // calculateMeter 
    1239  
    1240  
    1241 // ################################################################ 
    1242  
    1243 void updateFrameADK() { 
    1244  
    1245         #if DEBUG_OUTPUT 
    1246                 Serial.print("eegSignal: "); 
    1247                 Serial.print(eegSignal, DEC); 
    1248                 Serial.print(" | eegAttention: "); 
    1249                 Serial.print(eegAttention, DEC); 
    1250                 Serial.print(" | eegMeditation: "); 
    1251                 Serial.print(eegMeditation, DEC); 
    1252                 Serial.print(" | eegPower: "); 
    1253                 Serial.println(eegPower, DEC); 
    1254                 Serial.println(); 
    1255                  
    1256                 Serial.print("Thottle: "); 
    1257                 Serial.print(_throttle, DEC); 
    1258                 Serial.print(" | Yaw: "); 
    1259                 Serial.print(_yaw, DEC); 
    1260                 Serial.print(" | Pitch: "); 
    1261                 Serial.println(_pitch, DEC); 
    1262         #endif 
    1263          
    1264 //      setColorWheel(0, 0, 0); // black 
    1265          
    1266          
    1267         // if the signal is good enough, light 6:00 LED in green. 
    1268         if (eegSignal == 100) { 
    1269                 RGB_Panel[5][0]=0; 
    1270                 RGB_Panel[5][1]=255; 
    1271                 RGB_Panel[5][2]=0; 
    1272         } else { 
    1273                 RGB_Panel[5][0]=0; 
    1274                 RGB_Panel[5][1]=63; 
    1275                 RGB_Panel[5][2]=0; 
    1276                 // The following two lines can optionally be used 
    1277                 // to set all lights to red to indicate ADK mode 
    1278                 // when the EEG signal quality is insufficient for processing 
    1279 //              setColorWheel(255, 0, 0); // red 
    1280 //              return; 
    1281         } 
    1282          
    1283          
    1284         int attention = calculateMeter(eegAttention); 
    1285         int meditation = calculateMeter(eegMeditation); 
    1286          
    1287          
    1288                 //light up & dim red LED according to attention level 
    1289         for(int i=6; i<6+attention; i++) { 
    1290                 RGB_Panel[i][0]=255; 
    1291                 RGB_Panel[i][1]=0; 
    1292                 RGB_Panel[i][2]=0; 
    1293         } 
    1294         for(int i=6+attention; i<11; i++) { 
    1295                 RGB_Panel[i][0]=0; 
    1296                 RGB_Panel[i][1]=0; 
    1297                 RGB_Panel[i][2]=0; 
    1298         } 
    1299  
    1300         //light up & dim blue LED according to meditation level 
    1301         for(int i=4; i>4-meditation; i--) { 
    1302                 RGB_Panel[i][0]=0; 
    1303                 RGB_Panel[i][1]=0; 
    1304                 RGB_Panel[i][2]=255; 
    1305         } 
    1306         for(int i=4-meditation; i>-1; i--) { 
    1307                 RGB_Panel[i][0]=0; 
    1308                 RGB_Panel[i][1]=0; 
    1309                 RGB_Panel[i][2]=0; 
    1310         } 
    1311  
    1312  
    1313         if (eegPower > 0) { 
    1314                 RGB_Panel[11][0]=255; 
    1315                 RGB_Panel[11][1]=255; 
    1316                 RGB_Panel[11][2]=255; 
    1317 //              _throttle=_throttle_hover; 
    1318 //              Serial.println("Throttle On"); 
    1319         } else { 
    1320                 RGB_Panel[11][0]=0; 
    1321                 RGB_Panel[11][1]=0; 
    1322                 RGB_Panel[11][2]=0; 
    1323 //              _throttle=0; 
    1324 //              Serial.println("Throttle Off"); 
    1325         } // eegPower 
    1326          
    1327          
    1328         sendFrame(0); 
    1329  
    1330  
    1331 } // updateFrameADK() 
    1332  
    1333  
    1334 // ################################################################ 
    1335  
    1336 void parseADK() { 
    1337          
    1338         // Android ADK 
    1339          
    1340         if (acc.isConnected()) { 
    1341                  
    1342                 int len = acc.read(msg, sizeof(msg), 1); 
    1343                  
    1344                 if (! modeADK) { 
    1345                         modeADK = true; 
    1346                         modeBluetooth = false; 
    1347                         modeSerial = false; 
    1348                         Serial1.end(); 
    1349                         Serial.println("INFO: parseADK connection detected, setting modeADK"); 
    1350                         setColorWheel(255, 0, 0); // red 
    1351                 } 
    1352                  
    1353                 // Action taken by Arduino is tied to the message it receives from Android 
    1354                  
    1355                 if (len > 0) { 
    1356                          
    1357 //                      Serial.println("INFO: ADK message received"); 
    1358                          
    1359                         if (msg[0] == 0x1) { 
    1360 //                              Serial.println("0x1"); 
    1361                                 eegSignal = (int)msg[1]; 
    1362 //                              Serial.println(eegSignal); 
    1363                         } 
    1364                          
    1365                         else if(msg[0] == 0x2) { 
    1366                                 eegAttention = (int)msg[1]; 
    1367                         } 
    1368                          
    1369                         else if (msg[0] == 0x3) { 
    1370                                 eegMeditation = (int)msg[1]; 
    1371                         } 
    1372                          
    1373                         else if (msg[0] == 0x4) { 
    1374                                 eegPower = (int)msg[1]; 
    1375                         } 
    1376                          
    1377                         else if(msg[0] == 0x5) { 
    1378                                 _throttle = (int)msg[1]; 
    1379                         } 
    1380                          
    1381                         else if (msg[0] == 0x6) { 
    1382                                 _yaw = (int)msg[1]; 
    1383                         } 
    1384                          
    1385                         else if (msg[0] == 0x7) { 
    1386                                 _pitch = (int)msg[1]; 
    1387                         } 
    1388                          
    1389                         else if (msg[0] == 0x8) { 
    1390                                 if (msg[1] == 0x1) 
    1391                                         _channel = 'A'; 
    1392                                 else if (msg[1] == 0x2) 
    1393                                         _channel = 'B'; 
    1394                                 else if (msg[1] == 0x3) 
    1395                                         _channel = 'C'; 
    1396                         } 
    1397                          
    1398                          
    1399                         // Orbit Hover 
    1400                         if (msg[0] == 0x9) { 
    1401                                  
    1402                                 if (msg[1] == 0x1) { 
    1403                                         setColorWheel(255,255,255); // white 
    1404                                         _throttle = _throttle_hover; 
    1405                                         _yaw = DEFAULT_YAW; 
    1406                                         _pitch = DEFAULT_PITCH; 
    1407                                 } 
    1408                                 else if (msg[1] == 0x0) { 
    1409                                         setColorWheel(255,0,0); // red 
    1410                                         _throttle = DEFAULT_THROTTLE; 
    1411                                         _yaw = DEFAULT_YAW; 
    1412                                         _pitch = DEFAULT_PITCH; 
    1413                                 } 
    1414                                  
    1415                         } else { 
    1416                                 // Update the color wheel with all values if not forcing hover mode 
    1417                                 updateFrameADK(); 
    1418                         } // Hover 
    1419                          
    1420                  
    1421                 } // len 
    1422                  
    1423                 sendCode(formCode(_throttle,_yaw,_pitch)); 
    1424                 delay(80); //cycle(); 
    1425                  
    1426                  
    1427         } // if acc.isConnected() 
    1428  
    1429  
    1430 } // parseADK 
    1431  
    1432  
    1433  
    1434  
    1435  
    1436  
    1437  
    1438  
     1494 
     1495 
     1496 
     1497 
     1498 
     1499 
     1500 
     1501 
Note: See TracChangeset for help on using the changeset viewer.