Sam Shum / ros_mbed_base_controller

Sam

actiondrv.cpp@2:611a5eb132a1, 2020-09-13 (annotated)

Committer:
s0313045
Date:
Sun Sep 13 04:30:54 2020 +0000
Revision:
2:611a5eb132a1
Parent:
0:502b364c9f1d 
by Sam

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s0313045 0:502b364c9f1d 1 #include "mbed.h"
s0313045 0:502b364c9f1d 2 #include "actiondrv.h"
s0313045 0:502b364c9f1d 3
s0313045 0:502b364c9f1d 4 static char msgsetmode[] = {0x2B,0x60,0x60,0x03,0x00,0x00,0x00,0x00};
s0313045 0:502b364c9f1d 5 static char msgsetacc[] = {0x23,0x83,0x60,0x00,0x80,0x96,0x98,0x00};
s0313045 0:502b364c9f1d 6 static char msgsetdec[] = {0x23,0x84,0x60,0x00,0x80,0x96,0x98,0x00};
s0313045 0:502b364c9f1d 7 static char msgsetvel[] = {0x23,0xff,0x60,0x00,0x00,0x00,0x00,0x00};
s0313045 0:502b364c9f1d 8 //static char heartbeat[] = {0x2B,0x17,0x10,0x00,0x64,0x00,0x00,0x00};
s0313045 0:502b364c9f1d 9 static char msgenable[] = {0x01,0x00};
s0313045 0:502b364c9f1d 10 static char enablemotor[] = {0x2B,0x40,0x60,0x00,0x0F,0x00,0x00,0x00};
s0313045 0:502b364c9f1d 11 DigitalOut led1(LED1);
s0313045 0:502b364c9f1d 12 CAN can1(PA_11, PA_12, 500000);
s0313045 0:502b364c9f1d 13
s0313045 0:502b364c9f1d 14 int rpm_to_pulse_per_s = 500*4/60;
s0313045 0:502b364c9f1d 15
s0313045 0:502b364c9f1d 16 actionDrv::actionDrv(int _id)
s0313045 0:502b364c9f1d 17 {
s0313045 0:502b364c9f1d 18 id = _id;
s0313045 0:502b364c9f1d 19
s0313045 0:502b364c9f1d 20 }
s0313045 0:502b364c9f1d 21
s0313045 0:502b364c9f1d 22 void actionDrv::send(char* msg)
s0313045 0:502b364c9f1d 23 {
s0313045 0:502b364c9f1d 24 //if(can1.write(CANMessage(0x600+ id, msg)))
s0313045 0:502b364c9f1d 25 //{
s0313045 0:502b364c9f1d 26 // led1 = !led1;
s0313045 0:502b364c9f1d 27 // wait(0.05);
s0313045 0:502b364c9f1d 28 //}
s0313045 0:502b364c9f1d 29
s0313045 0:502b364c9f1d 30 while(!can1.write(CANMessage(0x600+ id, msg))) {wait(0.05);}
s0313045 0:502b364c9f1d 31 }
s0313045 0:502b364c9f1d 32
s0313045 0:502b364c9f1d 33
s0313045 0:502b364c9f1d 34 void actionDrv::Enable(){
s0313045 0:502b364c9f1d 35 //msgenable[1] = ((id >>24)& 0xFF);
s0313045 0:502b364c9f1d 36 //can1.write(CANMessage(0x000, msgenable));
s0313045 0:502b364c9f1d 37 //wait(0.2);
s0313045 0:502b364c9f1d 38 //send(enablemotor);
s0313045 0:502b364c9f1d 39
s0313045 0:502b364c9f1d 40 msgenable[1] = ((id >>24)& 0xFF);
s0313045 0:502b364c9f1d 41 while(!can1.write(CANMessage(0x000, msgenable)));
s0313045 0:502b364c9f1d 42 wait(0.2);
s0313045 0:502b364c9f1d 43 send(enablemotor);
s0313045 0:502b364c9f1d 44 }
s0313045 0:502b364c9f1d 45 void actionDrv::SetOperationalMode(){
s0313045 0:502b364c9f1d 46 send(msgsetmode);
s0313045 0:502b364c9f1d 47 }
s0313045 0:502b364c9f1d 48 void actionDrv::Configvelocity(int acc, int dec){
s0313045 0:502b364c9f1d 49
s0313045 0:502b364c9f1d 50 msgsetacc[4] = ((acc ) & 0xFF);
s0313045 0:502b364c9f1d 51 msgsetacc[5] = ((acc >> 8) & 0xFF);
s0313045 0:502b364c9f1d 52 msgsetacc[6] = ((acc >> 16) & 0xFF);
s0313045 0:502b364c9f1d 53 msgsetacc[7] = ((acc >> 24) & 0xFF);
s0313045 0:502b364c9f1d 54 msgsetdec[4] = ((dec ) & 0xFF);
s0313045 0:502b364c9f1d 55 msgsetdec[5] = ((dec >> 8) & 0xFF);
s0313045 0:502b364c9f1d 56 msgsetdec[6] = ((dec >> 16) & 0xFF);
s0313045 0:502b364c9f1d 57 msgsetdec[7] = ((dec >> 24) & 0xFF);
s0313045 0:502b364c9f1d 58
s0313045 0:502b364c9f1d 59 send(msgsetacc);
s0313045 0:502b364c9f1d 60 send(msgsetdec);
s0313045 0:502b364c9f1d 61 }
s0313045 0:502b364c9f1d 62 void actionDrv::SetVelocity(int vel){
s0313045 0:502b364c9f1d 63 //vel = vel*500*4/60; // rpm to pulse per s
s0313045 0:502b364c9f1d 64 vel = vel*rpm_to_pulse_per_s;
s0313045 0:502b364c9f1d 65 msgsetvel[4] = ((vel ) &0xFF);
s0313045 0:502b364c9f1d 66 msgsetvel[5] = ((vel >> 8) &0xFF);
s0313045 0:502b364c9f1d 67 msgsetvel[6] = ((vel >> 16) &0xFF);
s0313045 0:502b364c9f1d 68 msgsetvel[7] = ((vel >> 24) & 0xFF);
s0313045 0:502b364c9f1d 69 send(msgsetvel);
s0313045 0:502b364c9f1d 70 }
s0313045 0:502b364c9f1d 71
s0313045 0:502b364c9f1d 72 void actionDrv::send_mod(char* msg)
s0313045 0:502b364c9f1d 73 {
s0313045 0:502b364c9f1d 74 //if(can1.write(CANMessage(0x600+ id, msg)))
s0313045 0:502b364c9f1d 75 //{
s0313045 0:502b364c9f1d 76 // led1 = !led1;
s0313045 0:502b364c9f1d 77 //}
s0313045 0:502b364c9f1d 78
s0313045 0:502b364c9f1d 79 while(!can1.write(CANMessage(0x600+ id, msg)));
s0313045 0:502b364c9f1d 80 }
s0313045 0:502b364c9f1d 81
s0313045 0:502b364c9f1d 82 void actionDrv::SetVelocity_mod(int vel){
s0313045 0:502b364c9f1d 83 //vel = vel*500*4/60; // rpm to pulse per s
s0313045 0:502b364c9f1d 84 vel = vel*rpm_to_pulse_per_s;
s0313045 0:502b364c9f1d 85 msgsetvel[4] = ((vel ) &0xFF);
s0313045 0:502b364c9f1d 86 msgsetvel[5] = ((vel >> 8) &0xFF);
s0313045 0:502b364c9f1d 87 msgsetvel[6] = ((vel >> 16) &0xFF);
s0313045 0:502b364c9f1d 88 msgsetvel[7] = ((vel >> 24) & 0xFF);
s0313045 0:502b364c9f1d 89 send_mod(msgsetvel);
s0313045 0:502b364c9f1d 90 }
s0313045 0:502b364c9f1d 91
s0313045 0:502b364c9f1d 92
s0313045 0:502b364c9f1d 93
s0313045 0:502b364c9f1d 94 void actionDrv::stop(){
s0313045 0:502b364c9f1d 95 SetVelocity(0);
s0313045 0:502b364c9f1d 96
s0313045 0:502b364c9f1d 97 }
s0313045 0:502b364c9f1d 98
s0313045 0:502b364c9f1d 99