catchrobo2022 / Mbed 2 deprecated CatchroboGripper

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@1:a636d553c70a, 2022-07-21 (annotated)

Committer:
kageyuta
Date:
Thu Jul 21 13:46:47 2022 +0000
Revision:
1:a636d553c70a
Parent:
0:c725bf9715c4
Child:
2:042ea0741254
update;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MazeTaka 0:c725bf9715c4 1 #include "mbed.h"
MazeTaka 0:c725bf9715c4 2 #include "CAN_com.h"
MazeTaka 0:c725bf9715c4 3 #include "structs.h"
MazeTaka 0:c725bf9715c4 4
kageyuta 1:a636d553c70a 5 #define PULSE_MAX 2450 // us
kageyuta 1:a636d553c70a 6 #define PULSE_MIN 550 //us
kageyuta 1:a636d553c70a 7 #define PERIOD 20 // us
kageyuta 1:a636d553c70a 8 #define POS_MAX 3.14159265359f // rad
kageyuta 1:a636d553c70a 9 #define POS_MIN 0.0f // rad
kageyuta 1:a636d553c70a 10 #define M_PI 3.14159265359f // rad
MazeTaka 0:c725bf9715c4 11
kageyuta 1:a636d553c70a 12 #define CAN_ID 4
MazeTaka 0:c725bf9715c4 13
MazeTaka 0:c725bf9715c4 14 CAN can(PA_11,PA_12);
MazeTaka 0:c725bf9715c4 15 Ticker can_ticker;
MazeTaka 0:c725bf9715c4 16 Timer timer;
kageyuta 1:a636d553c70a 17
MazeTaka 0:c725bf9715c4 18 PwmOut led_blue(PB_4);
MazeTaka 0:c725bf9715c4 19 PwmOut led_red(PB_5);
kageyuta 1:a636d553c70a 20
kageyuta 1:a636d553c70a 21 PwmOut servo1(PA_1);
kageyuta 1:a636d553c70a 22 PwmOut servo2(PA_3);
MazeTaka 0:c725bf9715c4 23
MazeTaka 0:c725bf9715c4 24 ControllerStruct controller;
MazeTaka 0:c725bf9715c4 25
MazeTaka 0:c725bf9715c4 26 void moveServo(uint8_t id,double degree);
MazeTaka 0:c725bf9715c4 27 void readCANData();
MazeTaka 0:c725bf9715c4 28
kageyuta 1:a636d553c70a 29 bool received = false;
MazeTaka 0:c725bf9715c4 30
MazeTaka 0:c725bf9715c4 31 int main()
MazeTaka 0:c725bf9715c4 32 {
MazeTaka 0:c725bf9715c4 33 can.frequency(1000000);
MazeTaka 0:c725bf9715c4 34 can.filter(CAN_ID, 0xFFF, CANStandard, 0);
MazeTaka 0:c725bf9715c4 35 can.attach(&readCANData);
MazeTaka 0:c725bf9715c4 36
kageyuta 1:a636d553c70a 37 servo1.period_ms(PERIOD);
kageyuta 1:a636d553c70a 38 servo2.period_ms(PERIOD);
kageyuta 1:a636d553c70a 39
MazeTaka 0:c725bf9715c4 40 while(1) {
kageyuta 1:a636d553c70a 41 if (received == true){
kageyuta 1:a636d553c70a 42 double gripper_pos = controller.p_des;
kageyuta 1:a636d553c70a 43 int pulse1 = (PULSE_MAX - PULSE_MIN) * gripper_pos / (POS_MAX - POS_MIN) + PULSE_MIN;
kageyuta 1:a636d553c70a 44 int pulse2 = (PULSE_MAX - PULSE_MIN) * (M_PI - gripper_pos) / (POS_MAX - POS_MIN) + PULSE_MIN;
kageyuta 1:a636d553c70a 45 servo1.pulsewidth_us(pulse1);
kageyuta 1:a636d553c70a 46 servo2.pulsewidth_us(pulse2);
kageyuta 1:a636d553c70a 47 wait(0.01);
MazeTaka 0:c725bf9715c4 48 }
MazeTaka 0:c725bf9715c4 49 }
MazeTaka 0:c725bf9715c4 50 }
MazeTaka 0:c725bf9715c4 51
MazeTaka 0:c725bf9715c4 52 void readCANData()
MazeTaka 0:c725bf9715c4 53 {
MazeTaka 0:c725bf9715c4 54 CANMessage can_msg;
MazeTaka 0:c725bf9715c4 55 if(can.read(can_msg)) {
MazeTaka 0:c725bf9715c4 56 if(can_msg.id==CAN_ID) {
MazeTaka 0:c725bf9715c4 57 unpack_cmd(can_msg,&controller);
kageyuta 1:a636d553c70a 58 received = true;
MazeTaka 0:c725bf9715c4 59 }
MazeTaka 0:c725bf9715c4 60 }
kageyuta 1:a636d553c70a 61 }