Merge pull request #3 from roboticafacil/master

Servo Trim by Leopoldo
This commit is contained in:
Lukas Bachschwell 2018-04-18 21:21:11 +02:00 committed by GitHub
commit 9304bb2556
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 47 additions and 3 deletions

View File

@ -15,8 +15,14 @@ Blockly.Blocks['HIPPIE_init'] = {
this.appendValueInput('YL').appendField(new Blockly.FieldImage('img/blocks/hippie_YL.svg', 24*options.zoom, 24*options.zoom)).appendField(Facilino.locales.getKey('LANG_MOVEMENT_HIPPIE_YL')).appendField(new Blockly.FieldImage('img/blocks/pwm_signal.svg', 24*options.zoom, 24*options.zoom)).setAlign(Blockly.ALIGN_RIGHT); this.appendValueInput('YL').appendField(new Blockly.FieldImage('img/blocks/hippie_YL.svg', 24*options.zoom, 24*options.zoom)).appendField(Facilino.locales.getKey('LANG_MOVEMENT_HIPPIE_YL')).appendField(new Blockly.FieldImage('img/blocks/pwm_signal.svg', 24*options.zoom, 24*options.zoom)).setAlign(Blockly.ALIGN_RIGHT);
this.appendValueInput('RR').appendField(new Blockly.FieldImage('img/blocks/hippie_RR.svg', 24*options.zoom, 24*options.zoom)).appendField(Facilino.locales.getKey('LANG_MOVEMENT_HIPPIE_RR')).appendField(new Blockly.FieldImage('img/blocks/pwm_signal.svg', 24*options.zoom, 24*options.zoom)).setAlign(Blockly.ALIGN_RIGHT); this.appendValueInput('RR').appendField(new Blockly.FieldImage('img/blocks/hippie_RR.svg', 24*options.zoom, 24*options.zoom)).appendField(Facilino.locales.getKey('LANG_MOVEMENT_HIPPIE_RR')).appendField(new Blockly.FieldImage('img/blocks/pwm_signal.svg', 24*options.zoom, 24*options.zoom)).setAlign(Blockly.ALIGN_RIGHT);
this.appendValueInput('RL').appendField(new Blockly.FieldImage('img/blocks/hippie_RL.svg', 24*options.zoom, 24*options.zoom)).appendField(Facilino.locales.getKey('LANG_MOVEMENT_HIPPIE_RL')).appendField(new Blockly.FieldImage('img/blocks/pwm_signal.svg', 24*options.zoom, 24*options.zoom)).setAlign(Blockly.ALIGN_RIGHT); this.appendValueInput('RL').appendField(new Blockly.FieldImage('img/blocks/hippie_RL.svg', 24*options.zoom, 24*options.zoom)).appendField(Facilino.locales.getKey('LANG_MOVEMENT_HIPPIE_RL')).appendField(new Blockly.FieldImage('img/blocks/pwm_signal.svg', 24*options.zoom, 24*options.zoom)).setAlign(Blockly.ALIGN_RIGHT);
this.appendDummyInput('').appendField(Facilino.locales.getKey('LANG_MOVEMENT_HIPPIE_OFFSET'));
this.appendDummyInput('').appendField(Facilino.locales.getKey('LANG_MOVEMENT_HIPPIE_YR')).appendField('+/-').appendField(new Blockly.FieldTextInput('0'), 'OYR').setAlign(Blockly.ALIGN_RIGHT);
this.appendDummyInput('').appendField(Facilino.locales.getKey('LANG_MOVEMENT_HIPPIE_YL')).appendField('+/-').appendField(new Blockly.FieldTextInput('0'), 'OYL').setAlign(Blockly.ALIGN_RIGHT);
this.appendDummyInput('').appendField(Facilino.locales.getKey('LANG_MOVEMENT_HIPPIE_RR')).appendField('+/-').appendField(new Blockly.FieldTextInput('0'), 'ORR').setAlign(Blockly.ALIGN_RIGHT);
this.appendDummyInput('').appendField(Facilino.locales.getKey('LANG_MOVEMENT_HIPPIE_RL')).appendField('+/-').appendField(new Blockly.FieldTextInput('0'), 'ORL').setAlign(Blockly.ALIGN_RIGHT);
this.setPreviousStatement(true,'code'); this.setPreviousStatement(true,'code');
this.setNextStatement(true,'code'); this.setNextStatement(true,'code');
this.setInputsInline(false);
this.setColour(Facilino.LANG_COLOUR_MOVEMENT_WALK); this.setColour(Facilino.LANG_COLOUR_MOVEMENT_WALK);
this.setTooltip(Facilino.locales.getKey('LANG_HIPPIE_INIT_TOOLTIP')); this.setTooltip(Facilino.locales.getKey('LANG_HIPPIE_INIT_TOOLTIP'));
} }
@ -28,9 +34,29 @@ Blockly.Blocks['HIPPIE_init'] = {
var input_YL = Blockly.Arduino.valueToCode(this,'YL',Blockly.Arduino.ORDER_ATOMIC) || '25'; var input_YL = Blockly.Arduino.valueToCode(this,'YL',Blockly.Arduino.ORDER_ATOMIC) || '25';
var input_RR = Blockly.Arduino.valueToCode(this,'RR',Blockly.Arduino.ORDER_ATOMIC) || '17'; var input_RR = Blockly.Arduino.valueToCode(this,'RR',Blockly.Arduino.ORDER_ATOMIC) || '17';
var input_RL = Blockly.Arduino.valueToCode(this,'RL',Blockly.Arduino.ORDER_ATOMIC) || '16'; var input_RL = Blockly.Arduino.valueToCode(this,'RL',Blockly.Arduino.ORDER_ATOMIC) || '16';
var input_OYR = Blockly.Arduino.valueToCode(this,'OYR',Blockly.Arduino.ORDER_ATOMIC) || '0';
var input_OYL = Blockly.Arduino.valueToCode(this,'OYL',Blockly.Arduino.ORDER_ATOMIC) || '0';
var input_ORR = Blockly.Arduino.valueToCode(this,'ORR',Blockly.Arduino.ORDER_ATOMIC) || '0';
var input_ORL = Blockly.Arduino.valueToCode(this,'ORL',Blockly.Arduino.ORDER_ATOMIC) || '0';
Blockly.Arduino.definitions_['define_hippie_h'] = '#include <Hippie.h>'; Blockly.Arduino.definitions_['define_hippie_h'] = '#include <Hippie.h>';
Blockly.Arduino.definitions_['declare_var_define_hippie']='Hippie hippie;\n'; Blockly.Arduino.definitions_['declare_var_define_hippie']='Hippie hippie;\n';
Blockly.Arduino.setups_['setup_hippie_init'] = 'hippie.init('+input_YR+','+input_YL+','+input_RR+','+input_RL+');\n'; Blockly.Arduino.setups_['setup_hippie_init'] = 'hippie.init_with_trim('+input_YR+','+input_YL+','+input_RR+','+input_RL+','+input_OYR+','+input_OYL+','+input_ORR+','+input_ORL+');\n';
return code;
};
Blockly.Arduino['HIPPIE_init'] = function(block) {
var code='';
var input_YR = Blockly.Arduino.valueToCode(this,'YR',Blockly.Arduino.ORDER_ATOMIC) || '26';
var input_YL = Blockly.Arduino.valueToCode(this,'YL',Blockly.Arduino.ORDER_ATOMIC) || '25';
var input_RR = Blockly.Arduino.valueToCode(this,'RR',Blockly.Arduino.ORDER_ATOMIC) || '17';
var input_RL = Blockly.Arduino.valueToCode(this,'RL',Blockly.Arduino.ORDER_ATOMIC) || '16';
var input_OYR = Blockly.Arduino.valueToCode(this,'OYR',Blockly.Arduino.ORDER_ATOMIC) || '0';
var input_OYL = Blockly.Arduino.valueToCode(this,'OYL',Blockly.Arduino.ORDER_ATOMIC) || '0';
var input_ORR = Blockly.Arduino.valueToCode(this,'ORR',Blockly.Arduino.ORDER_ATOMIC) || '0';
var input_ORL = Blockly.Arduino.valueToCode(this,'ORL',Blockly.Arduino.ORDER_ATOMIC) || '0';
Blockly.Arduino.definitions_['define_hippie_h'] = '#include <Hippie.h>';
Blockly.Arduino.definitions_['declare_var_define_hippie']='Hippie hippie;\n';
Blockly.Arduino.setups_['setup_hippie_init'] = 'hippie.init_with_trim('+input_YR+','+input_YL+','+input_RR+','+input_RL+','+input_OYR+','+input_OYL+','+input_ORR+','+input_ORL+');\n';
return code; return code;
}; };

View File

@ -4,14 +4,31 @@
#include <Oscillator.h> #include <Oscillator.h>
void Hippie::init(int YL, int YR, int RL, int RR) { void Hippie::init(int YL, int YR, int RL, int RR) {
servo_pins[0] = YL; servo_pins[0] = YL;
servo_pins[1] = YR; servo_pins[1] = YR;
servo_pins[2] = RL; servo_pins[2] = RL;
servo_pins[3] = RR; servo_pins[3] = RR;
attachServos();
isHippieResting=false;
for (int i = 0; i < 4; i++) servo_position[i] = 90;
}
void Hippie::init_with_trim(int YL, int YR, int RL, int RR, int OYL, int OYR, int ORL, int ORR) {
servo_pins[0] = YL;
servo_pins[1] = YR;
servo_pins[2] = RL;
servo_pins[3] = RR;
servo[0].SetTrim(OYL);
servo[1].SetTrim(OYR);
servo[2].SetTrim(ORL);
servo[3].SetTrim(ORR);
attachServos(); attachServos();
isHippieResting=false; isHippieResting=false;
@ -71,6 +88,7 @@ void Hippie::oscillateServos(int A[4], int O[4], int T, double phase_diff[4], fl
servo[i].SetA(A[i]); servo[i].SetA(A[i]);
servo[i].SetT(T); servo[i].SetT(T);
servo[i].SetPh(phase_diff[i]); servo[i].SetPh(phase_diff[i]);
servo[i].SetTrim(servo_trim[i]);
} }
double ref=millis(); double ref=millis();
for (double x=ref; x<=T*cycle+ref; x=millis()){ for (double x=ref; x<=T*cycle+ref; x=millis()){

View File

@ -19,7 +19,7 @@ class Hippie
//-- Hippie initialisation //-- Hippie initialisation
void init(int YL, int YR, int RL, int RR); void init(int YL, int YR, int RL, int RR);
void init_with_trim(int YL, int YR, int RL, int RR, int OYL, int OYR, int ORL, int ORR);
//-- Attach & detach functions //-- Attach & detach functions
void attachServos(); void attachServos();
void detachServos(); void detachServos();