|
|
| 第192行： |
第192行： |
| | | | |
| | |} | | |} |
| − |
| |
| − |
| |
| − | #include <mCookieLEGO.h>
| |
| − | #include <Microduino_Protocol_HardSer.h>
| |
| − | #include "Wire.h"
| |
| − |
| |
| − |
| |
| − | uint16_t Data[8];
| |
| − | int speedLeft = 0;
| |
| − | int speedRight = 0;
| |
| − | Protocol ProtocolB(&Serial1, TYPE_NUM);
| |
| − | mCookieLEGO m;
| |
| − |
| |
| − | void setup(){
| |
| − | Wire.begin();
| |
| − | Serial.begin(9600);
| |
| − |
| |
| − | ProtocolB.begin(9600); //9600/19200/38400
| |
| − | m.configSensorType(mCookieLEGO::S1, mCookieLEGO::SENSOR_ULTRA);
| |
| − | m.configConnection(0xFFFF);
| |
| − | }
| |
| − | uint8_t dis;
| |
| − |
| |
| − | void loop(){
| |
| − | int sta = ProtocolB.parse(Data, MODE_WHILE);
| |
| − | if (sta != P_NONE) {
| |
| − | switch (sta) {
| |
| − | case P_FINE:
| |
| − | speedLeft = map(Data[1], 1000, 2000, -255, 255);
| |
| − | speedRight = speedLeft;
| |
| − | int speedTurn = map(Data[2], 1000, 2000, -255, 255);
| |
| − | if(speedTurn < 0){
| |
| − | speedLeft += speedTurn;
| |
| − | }
| |
| − | else{
| |
| − | speedRight -= speedTurn;
| |
| − | }
| |
| − | refreshSpeed();
| |
| − | break;
| |
| − | // default:
| |
| − | // break;
| |
| − | }
| |
| − | }
| |
| − |
| |
| − | delay(10);
| |
| − | }
| |
| − |
| |
| − | void refreshSpeed(){
| |
| − | m.motor_setFixedDrive(mCookieLEGO::M3, speedLeft);
| |
| − | m.motor_setFixedDrive(mCookieLEGO::M4, speedRight);
| |
| − | Serial.print("Left: ");
| |
| − | Serial.print(speedLeft);
| |
| − | Serial.print(" Right: ");
| |
| − | Serial.println(speedRight);
| |
| − | }
| |
