Linear driver with st driver
Dependencies: X_NUCLEO_IHM04A1
Dependents: Basic_DC_Control Basic_DC_Control1 DC_Serial
Diff: main.cpp
- Revision:
- 26:c18d6aaa474a
- Parent:
- 25:84d2bd93bf97
- Child:
- 27:bf0109fb61c3
--- a/main.cpp Wed Jun 12 16:03:30 2019 +0000 +++ b/main.cpp Wed Jun 19 16:29:11 2019 +0000 @@ -1,7 +1,10 @@ #include "mbed.h" #include "L6206.h" -#define MAX_MOTOR (2) +#define JOINT_SET_SPEED 20 + +#define JOINT_ID 1 + static volatile uint16_t gLastError; static volatile uint8_t gStep = 0; @@ -20,7 +23,7 @@ }; L6206 *motor; -InterruptIn my_button_irq(USER_BUTTON); /* User button on Nucleo board */ + Thread canrxa; //Utility @@ -82,108 +85,41 @@ } } } -void end0_int_handler(unsigned int motorId) -{ - printf("END0: Pressed\n\rSPEED: %d\n\r", motor->get_speed(motorId)); -} - -void end1_int_handler() -{ - motor->hard_stop(0); - motor->hard_stop(1); //or hard_hiz(); for disabling the bridge? - - motor->run(0, BDCMotor::BWD); - motor->run(1, BDCMotor::BWD); - - printf("END1: Pressed\n\r"); -} - - // CAN, to revise CAN can1(PB_12, PB_13); // RX, TX CANMessage messageIn; -CANMessage messageOut; - - -int filter = can1.filter(0x010, 0x4FF, CANStandard); void canrx() { while(1) { - if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==6))) //Primo motore - { - speed=messageIn.data[0]; //Messaggio da 0 a 255, devo sottrrarre 127 e imporre velocità con segno - float speedMap=(speed-127)/127*100; - printf("CAN: mess %f\n\r", speedMap); - - //CAN MESSAGE WITH SPEED TO REVISE - //Ci sarebbe anche il set speed. - if (speedMap == 0) - { - motor->set_speed(0,0); //There's no soft stop. could it work like this? - //current_speed= motor->get_speed(0); We could do lie this? - //motor->go_to(current_pose); - } - else if (speedMap>0) - { - motor->run(0,BDCMotor::FWD); - motor->set_speed(0,(unsigned int) speedMap); - } - else if (speedMap<0) - { - motor->run(0,BDCMotor::BWD); - motor->set_speed(0,(unsigned int) -speedMap); - } - else - { - motor->set_speed(0,0); //Riportare errore? NO - } - } - else if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==7))) //Secondo motore + if(can1.read(messageIn)) { - speed=messageIn.data[0]; //Messaggio da 0 a 255, devo sottrrarre 127 e imporre velocità con segno - float speedMap=(speed-127)/127*100; - printf("CAN: mess %f\n\r", speedMap); - - //CAN MESSAGE WITH SPEED TO REVISE - //Ci sarebbe anche il set speed. - if (speedMap == 0) + if(messageIn.id == ((JOINT_SET_SPEED << 8) + JOINT_ID)) { - motor->set_speed(1,0); //There's no soft stop. could it work like this? - //current_speed= motor->get_speed(0); We could do lie this? - //motor->go_to(current_pose); + speed = 0; + speed = (messageIn.data[0] << 24) | (messageIn.data[1] << 16) | (messageIn.data[2] << 8) | (messageIn.data[3]); + + motor->set_speed(0, speed); + (speed > 0) ? motor->run(0, BDCMotor::BWD) : motor->run(0, BDCMotor::FWD); + + printf("CAN: mess %d\n\r", speed); } - else if (speedMap>0) - { - motor->run(1,BDCMotor::FWD); - motor->set_speed(1,(unsigned int) speedMap); - } - else if (speedMap<0) - { - motor->run(1,BDCMotor::BWD); - motor->set_speed(1,(unsigned int) -speedMap); - } - else - { - motor->set_speed(0,0); //Riportare errore? NO - } - } - } + } + + wait(0.1); + } } -//Aggiungere un get speed,FATTO ci sarebbe anche il set_speed - //Dentro la libreria controllare se c'è controllo di Duty cycle FATTO - //motor->go_to(current_pose); + /* Main ----------------------------------------------------------------------*/ - int main() { - can1.frequency(125000); + can1.frequency(500000); messageIn.format=CANExtended; - messageOut.format=CANExtended; + // Motor Initialization #ifdef TARGET_STM32F429 @@ -216,6 +152,6 @@ while(true) { - wait(1000); + wait(1); } }