Team DIANA
SHOULDER
Dependencies: X-NUCLEO-IHM05A1
Diff: main.cpp
- Revision:
- 24:37f139e067b2
- Parent:
- 23:3110010d98a0
- Child:
- 25:281c8e913db4
--- a/main.cpp Sat Apr 06 14:51:15 2019 +0000
+++ b/main.cpp Wed Jun 12 15:53:00 2019 +0000
@@ -1,152 +1,209 @@
#include "mbed.h"
-#include "L6208.h"
+#include "L6206.h"
+
+#define MAX_MOTOR (2)
-#define VREFA_PWM_PIN D3
-#define VREFB_PWM_PIN D9
+static volatile uint16_t gLastError;
+static volatile uint8_t gStep = 0;
-l6208_init_t init =
+int current_pose = 0;
+int speed = 0;
+
+L6206_init_t init =
{
- 8000, //Acceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
- 80, //Acceleration current torque in % (from 0 to 100)
- 8000, //Deceleration rate in step/s^2 or (1/16)th step/s^2 for microstep modes
- 80, //Deceleration current torque in % (from 0 to 100)
- 8000, //Running speed in step/s or (1/16)th step/s for microstep modes
- 80, //Running current torque in % (from 0 to 100)
- 40, //Holding current torque in % (from 0 to 100)
- STEP_MODE_1_16, //Step mode via enum motorStepMode_t
- FAST_DECAY, //Decay mode via enum motorDecayMode_t
- 0, //Dwelling time in ms
- FALSE, //Automatic HIZ STOP
- 100000 //VREFA and VREFB PWM frequency (Hz)
+ L6206_CONF_PARAM_PARALLE_BRIDGES,
+ {L6206_CONF_PARAM_FREQ_PWM1A, L6206_CONF_PARAM_FREQ_PWM2A, L6206_CONF_PARAM_FREQ_PWM1B, L6206_CONF_PARAM_FREQ_PWM2B},
+ {100,100,100,100},
+ {FORWARD,FORWARD,BACKWARD,FORWARD},
+ {INACTIVE,INACTIVE,INACTIVE,INACTIVE},
+ {FALSE,FALSE}
};
+L6206 *motor;
+InterruptIn my_button_irq(USER_BUTTON); /* User button on Nucleo board */
Thread canrxa;
-// Utility
+//Utility
InterruptIn button(USER_BUTTON);
-DigitalOut led(LED1);
-
-// Motor Control
-L6208 *motor;
+DigitalOut led(LED1); //Change?
-InterruptIn end0(PC_10, PullUp);
-InterruptIn end1(PC_11, PullUp);
-InterruptIn enc(PC_12, PullUp);
-
-int current_pose = 0;
-int pose = 0;
void motor_error_handler(uint16_t error)
{
printf("ERROR: Motor Runtime\n\r");
- while(1){}
+ while(1){};
}
void motor_zero()
{
- motor->run(StepperMotor::FWD);
+ motor->run(0, BDCMotor::FWD);
+ motor->run(1, BDCMotor::FWD);
}
-void button_int_handler()
+void button_int_handler(unsigned int motorId)
{
- printf("POSITION: %d\n\r", motor->get_position());
+ printf("MOTOR SPEED: %d\n\r", motor->get_speed(motorId));
motor_zero();
}
-void end0_int_handler()
+void my_error_handler(uint16_t error)
+{
+ /* Backup error number */
+ gLastError = error;
+
+ /* Enter your own code here */
+}
+
+void my_flag_irq_handler(void)
{
- printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
+ /* Code to be customised */
+ /************************/
+ /* Get the state of bridge A */
+ uint16_t bridgeState = motor->get_bridge_status(0);
+
+ if (bridgeState == 0) {
+ if ((motor->get_device_state(0) != INACTIVE)||
+ (motor->get_device_state(1) != INACTIVE)) {
+ /* Bridge A was disabling due to overcurrent or over temperature */
+ /* When at least on of its motor was running */
+ my_error_handler(0XBAD0);
+ }
+ }
+
+ /* Get the state of bridge B */
+ bridgeState = motor->get_bridge_status(1);
+
+ if (bridgeState == 0) {
+ if ((motor->get_device_state(2) != INACTIVE)||
+ (motor->get_device_state(3) != INACTIVE)) {
+ /* Bridge A was disabling due to overcurrent or over temperature */
+ /* When at least on of its motor was running */
+ my_error_handler(0XBAD1);
+ }
+ }
+}
+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();
+ motor->hard_stop(0);
+ motor->hard_stop(1); //or hard_hiz(); for disabling the bridge?
- motor->run(StepperMotor::BWD);
+ motor->run(0, BDCMotor::BWD);
+ motor->run(1, BDCMotor::BWD);
printf("END1: Pressed\n\r");
}
-void motor_set_home()
-{
- motor->hard_stop();
- motor->set_home();
- motor->go_to(0);
-
- current_pose = 0;
- pose = 0;
-}
+
-// CAN
+// 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==0x010)
+ if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==6))) //Primo motore
{
- pose=messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
- printf("CAN: mess %d\n\r", pose);
+ 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 %d\n\r", speedMap, "\% ");
-
- if (pose == 1)
+ //CAN MESSAGE WITH SPEED TO REVISE
+ //Ci sarebbe anche il set speed.
+ if (speedMap == 0)
{
- motor->run(StepperMotor::FWD);
+ 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 (pose == 0)
+ else if (speedMap>0)
+ {
+ motor->run(0,BDCMotor::FWD);
+ motor->set_speed(0,(unsigned int) speedMap);
+ }
+ else if (speedMap<0)
{
- motor->soft_stop();
- current_pose= motor->get_position();
- motor->go_to(current_pose);
- }
- else if (pose == 2)
- {
- motor->run(StepperMotor::BWD);
+ motor->run(0,BDCMotor::BWD);
+ motor->set_speed(0,(unsigned int) -speedMap);
}
else
{
- motor->soft_stop();
- current_pose= motor->get_position();
- motor->go_to(current_pose);
+ motor->set_speed(0,0); //Riportare errore? NO
}
-
+ }
+ else if(can1.read(messageIn,filter)&& ((messageIn.id>>8 == 20) && (messageIn.id & 0x00FF==7))) //Secondo 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 %d\n\r", speedMap, "\% ");
- // motor->go_to(pose);
- }
- }
+ //CAN MESSAGE WITH SPEED TO REVISE
+ //Ci sarebbe anche il set speed.
+ if (speedMap == 0)
+ {
+ 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);
+ }
+ 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
+ }
+ }
}
-
+//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);
-
+ messageIn.format=CANExtended;
+ messageOut.format=CANExtended;
// Motor Initialization
- motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
- if (motor->init(&init) != COMPONENT_OK)
+#ifdef TARGET_STM32F429
+ motor = new L6206(D2, A4, PB_4, PC_7, PA_15, PB_3);
+#else
+ motor = new L6206(D2, A4, D5, D4, A0, A1);
+#endif
+
+ if (motor->init(&init) != COMPONENT_OK)
{
printf("ERROR: vvMotor Init\n\r");
exit(EXIT_FAILURE);
}
- motor->attach_error_handler(&motor_error_handler);
-
- end0.rise(&end0_int_handler);
- end1.rise(&end1_int_handler);
- end1.fall(&motor_set_home);
+ motor->attach_flag_interrupt(my_flag_irq_handler);
+ motor->attach_error_handler(my_error_handler);
+
+
button.rise(&button_int_handler);
- motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
printf("DONE: Motor Init\n\r");
// CAN Initialization