Team DIANA
SHOULDER
Dependencies: X-NUCLEO-IHM05A1
Revision 34:4be6353f0186, committed 2019-09-13
- Comitter:
- stebonicelli
- Date:
- Fri Sep 13 20:40:50 2019 +0000
- Parent:
- 33:48196cd5c052
- Commit message:
- Post dinner revision
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 48196cd5c052 -r 4be6353f0186 main.cpp
--- a/main.cpp Fri Sep 13 15:26:06 2019 +0000
+++ b/main.cpp Fri Sep 13 20:40:50 2019 +0000
@@ -4,23 +4,24 @@
#define VREFA_PWM_PIN D3
#define VREFB_PWM_PIN D9
#define SEND_FREQUENCY 10 //Hz
+
l6208_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)
+ 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)
};
+
// CAN
-
CAN can1(PA_11, PA_12); // RX, TX
CANMessage messageIn;
@@ -42,28 +43,28 @@
typedef enum
{
- JOINT_SET_SPEED = 20,
- JOINT_SET_POSITION,
- JOINT_CURRENT_POSITION,
- JOINT_CURRENT_SPEED,
- JOINT_STATUS,
- JOINT_ERROR,
- JOINT_TORQUE,
- JOINT_MAXTORQUE,
- JOINT_ZERO,
+ JOINT_SET_SPEED = 20,
+ JOINT_SET_POSITION,
+ JOINT_CURRENT_POSITION,
+ JOINT_CURRENT_SPEED,
+ JOINT_STATUS,
+ JOINT_ERROR,
+ JOINT_TORQUE,
+ JOINT_MAXTORQUE,
+ JOINT_ZERO,
}CAN_COMMANDS;
typedef enum
{
- BASE=1,
- SHOULDER,
- ELBOW,
- WRIST1,
- WRIST2,
- WRIST3,
- END_EFFECTOR,
- CAMERA1,
- CAMERA2,
+ BASE=1,
+ SHOULDER,
+ ELBOW,
+ WRIST1,
+ WRIST2,
+ WRIST3,
+ END_EFFECTOR,
+ CAMERA1,
+ CAMERA2,
}JOINT;
float pose, current_pose;
@@ -71,140 +72,141 @@
void set_zero()
{
- printf("set zero\t\n");
- motor->hard_stop();
- motor->set_home();
- motor->go_to(0);
+ printf("set zero\t\n");
+
+ motor->hard_stop();
+ motor->set_home();
+ motor->go_to(0);
}
+
void zero()
{
printf("zero");
motor->run(StepperMotor::FWD);
-
}
-
-
+
uint32_t gen_can_id(CAN_COMMANDS message_id, JOINT can_id)
{
- uint32_t id = (uint32_t)can_id; // LSB byte is the controller id.
- id |= (uint32_t)message_id << 8; // Next lowest byte is the packet id.
- id |= 0x80000000; // Send in Extended Frame Format.
- return id;
+ uint32_t id = (uint32_t)can_id; // LSB byte is the controller id.
+ id |= (uint32_t)message_id << 8; // Next lowest byte is the packet id.
+ id |= 0x80000000; // Send in Extended Frame Format.
+ return id;
}
+
double to_rad(double angle)
{
- return angle*0.0174533;
+ return angle*0.0174533;
}
+
double angle_deparse (long int pose, float offset)
{
- offset = offset * 0.00872664625;
- double angle = pose *0.000487012987; //do something 0,0004791666667
- angle = (angle - offset);
- return angle;
+ offset = offset * 0.00872664625;
+ double angle = pose *0.000487012987; //do something 0,0004791666667
+ angle = (angle - offset);
+ return angle;
}
+
void motor_error_handler(uint16_t error)
{
- printf("ERROR: Motor Runtime\n\r");
+ printf("ERROR: Motor Runtime\n\r");
+}
-}
void end0_int_handler()
{
- motor->hard_stop();
+ motor->hard_stop();
+ motor->run(StepperMotor::BWD);
+
+ printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
+}
- motor->run(StepperMotor::BWD);
-
- printf("END0: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
-}
void end0_int_handler_fall()
{
- motor->hard_stop();
- int position = motor->get_position();
- motor->go_to(position);
-
-
+ motor->hard_stop();
+ int position = motor->get_position();
+ motor->go_to(position);
}
void end1_int_handler()
{
- motor->hard_stop();
-
- motor->run(StepperMotor::FWD);
-
- printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
+ motor->hard_stop();
+ motor->run(StepperMotor::FWD);
+
+ printf("END1: Pressed\n\rPOSITION: %d\n\r", motor->get_position());
}
-
-
void cantx ()
{
while(1)
{
-
- int _pose;
- messageOut.format = CANExtended;
- messageOut.id=gen_can_id(JOINT_CURRENT_POSITION, SHOULDER);
- pose=angle_deparse(motor->get_position(), 0);
- _pose=pose*100;
- messageOut.data[3]=_pose;
- messageOut.data[2]=_pose >>8;
- messageOut.data[1]=_pose >>16;
- messageOut.data[0]=_pose >>24;
-
- int status = can1.write(messageOut);
- led=status;
- printf("CAN send CURRENT POSITION Joint status %d : pose %f\t\n",status, pose);
+ int _pose;
+
+ messageOut.format = CANExtended;
+ messageOut.id=gen_can_id(JOINT_CURRENT_POSITION, SHOULDER);
+
+ pose=angle_deparse(motor->get_position(), 0);
+ _pose=pose*100;
+
+ messageOut.data[3]=_pose;
+ messageOut.data[2]=_pose >>8;
+ messageOut.data[1]=_pose >>16;
+ messageOut.data[0]=_pose >>24;
+
+ int status = can1.write(messageOut);
+ printf("CAN send CURRENT POSITION Joint status %d : pose %f\t\n",status, pose);
}
}
+
void cantx_ISR()
{
cantx();
osDelay(1/SEND_FREQUENCY*1000);
}
-
void canrx()
{
- while(1)
- {
-
- if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_SET_SPEED,SHOULDER))
- {
- speed=messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24);
- printf("CAN: mess received : %f\n\r", speed);
- current_speed=speed;
-
- led = !led;
+ while(1)
+ {
+ if(can1.read(messageIn))
+ {
+ led = !led;
+
+ printf("Read!\n\r");
+
+ if(messageIn.id==gen_can_id(JOINT_SET_SPEED,SHOULDER))
+ {
+ speed=messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24);
+ printf("CAN: mess received : %f\n\r", speed);
+ current_speed=speed;
+
+ if (current_speed>0)
+ {
+ motor->set_max_speed(current_speed*80);
+ motor->run(StepperMotor::FWD);
+ }
+ else if (current_speed<0)
+ {
+ motor->set_max_speed(current_speed*80);
+ motor->run(StepperMotor::BWD);
+ }
+ else
+ {
+ motor->soft_stop();
+ current_pose= motor->get_position();
+ motor->go_to(current_pose);
+ }
+ }
+ else if(messageIn.id==gen_can_id(JOINT_ZERO,SHOULDER))
+ {
+ if((messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24))==1)
+ {
+ zero();
+ motor->wait_while_active();
+ }
+ }
+ }
+ }
+}
- if (current_speed>0)
- {
- motor->set_max_speed(current_speed*80);
- motor->run(StepperMotor::FWD);
- }
- else if (current_speed<0)
- {
- motor->set_max_speed(current_speed*80);
- motor->run(StepperMotor::BWD);
- }
-
- else
- {
- motor->soft_stop();
- current_pose= motor->get_position();
- motor->go_to(current_pose);
- }
- }
-
- if(can1.read(messageIn)&&messageIn.id==gen_can_id(JOINT_ZERO,SHOULDER))
- {
- if((messageIn.data[3] + (messageIn.data[2] << 8) + (messageIn.data[1] << 16) + (messageIn.data[0] << 24))==1)
- {
- zero();
- motor->wait_while_active();
- }
- }
-
- }
-}
void canrx_ISR()
{
canrx();
@@ -215,39 +217,42 @@
int main()
{
-
- can1.frequency(125000);
-
- // Motor Initialization
- motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
- motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
- 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);
- end0.fall(&end0_int_handler_fall);
- end1.rise(&end1_int_handler);
- end1.fall(&set_zero);
-
- printf("DONE: Motor Init\n\r");
-
- // CAN Initialization
- // zero();
+
+ can1.frequency(125000);
+
+ // Motor Initialization
+ motor = new L6208(D2, D8, D7, D4, D5, D6, VREFA_PWM_PIN, VREFB_PWM_PIN);
+ motor->set_step_mode(StepperMotor::STEP_MODE_1_16);
+
+ 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);
+ end0.fall(&end0_int_handler_fall);
+ end1.rise(&end1_int_handler);
+ end1.fall(&set_zero);
- canrxa.start(canrx_ISR);
- //cantxa.start(cantx_ISR);
-
- printf("DONE: CAN Init\n\r");
-
- printf("Running!\n\r");
- //zero();
- while(true)
- {
- wait(1000);
- }
+ printf("DONE: Motor Init\n\r");
+
+ // CAN Initialization
+ // zero();
+
+ canrxa.start(canrx_ISR);
+ //cantxa.start(cantx_ISR);
+
+ printf("DONE: CAN Init\n\r");
+
+ printf("Running!\n\r");
+
+ //zero();
+
+ while(true)
+ {
+ wait(1000);
+ }
}