Hi
I’m trying to limit the rotation of the arm so that it won’t hit the MegaPi and to prevent stress on the encoder motor connected to it. I’m trying to make use of y-axis of gyro sensor, encoder motor port and signal sent over bluetooth via Serial3. So far I have my code implemented (the #s part) in the MegaPi firmware file (the source file) but it doesn’t work as intended.
If user exceeds the safety range, I want the user to still be able to move the arm back to safe zone. I’ve been studying the firmware code but still don’t have a clue of how to implement my code properly. Please help.
void loop()
{ double range = gyro_ext.getAngleY();
gyro_ext.update();
//Serial.read();
//Serial.print(“X:”);
//Serial.print(gyro_ext.getAngleX() );
//Serial.print(" Y:");
//Serial.println(range);
//Serial.print(" Z:");
//Serial.println(gyro_ext.getAngleZ() );
delay(10);
currentTime = millis()/1000.0-lastTime;
keyPressed = buttonSensor.pressed();
if(millis() - blink_time > 1000)
{
blink_time = millis();
blink_flag = !blink_flag;
digitalWrite(13,blink_flag);
}
Encoder_1.loop();
Encoder_2.loop();
//#########################################################################################
uint8_t slot_num = readBuffer(7);
/Serial.print("slot_num: ");
Serial.println(slot_num);/
if(((range > 35.00) || (range < -9.00)) && slot_num == SLOT_3){
if((range > 35.00) && ( (Serial3.read() >= 156) && (Serial3.read() <= 255) )){//beyond positive range and user specify to go down
Encoder_3.loop();
Serial.println(“Moving down”);
}else if((range < -9.00) && (Serial3.read() >= 0) && (Serial3.read() <= 100)){
Encoder_3.loop();
Serial.println(“Moving up”);
}else{
if(Serial3.read()){
//Serial.println(“NOT POSSIBLE”);
}
/if((range < -9.00) && (Serial3.read() > 0)){
Encoder_3.loop();
Serial.println(“Moving up”);
}/
Encoder_3.runSpeed(0);
}
}else if(((range < 35.00) && (range > -9.00)) && slot_num == SLOT_3){//safe zone
Encoder_3.loop();
Serial.println("+++++Moving+++++");
}else{
//do nothing
}
//#########################################################################################
Encoder_4.loop();
// while(Serial.available() > 0)
// {
// char c = Serial.read();
// Serial.write©;
// buf[bufindex++]=c;
// if((c==’\n’) || (c==’#’))
// {
// parseCmd(buf);
// memset(buf,0,64);
// bufindex = 0;
// }
// }
readSerial();
while(isAvailable)
{
unsigned char c = serialRead & 0xff;
if((c == 0x55) && (isStart == false))
{
if(prevc == 0xff)
{
index=1;
isStart = true;
}
}
else
{
prevc = c;
if(isStart)
{
if(index == 2)
{
dataLen = c;
}
else if(index > 2)
{
dataLen–;
}
writeBuffer(index,c);
}
}
index++;
if(index > 51)
{
index=0;
isStart=false;
}
if(isStart && (dataLen == 0) && (index > 3))
{
isStart = false;
Serial.print("In_LOOP_SERIAL3: ");
Serial.println(Serial3.read());
parseData();
index=0;
}
readSerial();
}
gyro_ext.fast_update();
if(Compass.getPort() != 0)
{
Compass.getAngle();
}
angle_speed = gyro_ext.getGyroY();
if(megapi_mode == BLUETOOTH_MODE)
{
//Serial.println(“blueeeeee”);
}
else if(megapi_mode == BALANCED_MODE)
{
Serial.println(“BALANCED??”);
balanced_model();
}
}