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@2:042ea0741254, 2022-07-26 (annotated)

Committer:
qwertyaz
Date:
Tue Jul 26 02:52:56 2022 +0000
Revision:
2:042ea0741254
Parent:
1:a636d553c70a 
test

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
qwertyaz 2:042ea0741254 5 #define PULSE_MAX 2400 // us
qwertyaz 2:042ea0741254 6 #define PULSE_MIN 600 //us
kageyuta 1:a636d553c70a 7 #define PERIOD 20 // us
qwertyaz 2:042ea0741254 8 #define POS_MAX 3.14159265359f // radx
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
qwertyaz 2:042ea0741254 21 PwmOut servo1(PA_3);
qwertyaz 2:042ea0741254 22 PwmOut servo2(PA_1);
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);
MazeTaka 0:c725bf9715c4 39 while(1) {
kageyuta 1:a636d553c70a 40 if (received == true){
kageyuta 1:a636d553c70a 41 double gripper_pos = controller.p_des;
kageyuta 1:a636d553c70a 42 int pulse1 = (PULSE_MAX - PULSE_MIN) * gripper_pos / (POS_MAX - POS_MIN) + PULSE_MIN;
kageyuta 1:a636d553c70a 43 int pulse2 = (PULSE_MAX - PULSE_MIN) * (M_PI - gripper_pos) / (POS_MAX - POS_MIN) + PULSE_MIN;
kageyuta 1:a636d553c70a 44 servo1.pulsewidth_us(pulse1);
kageyuta 1:a636d553c70a 45 servo2.pulsewidth_us(pulse2);
kageyuta 1:a636d553c70a 46 wait(0.01);
MazeTaka 0:c725bf9715c4 47 }
MazeTaka 0:c725bf9715c4 48 }
MazeTaka 0:c725bf9715c4 49 }
MazeTaka 0:c725bf9715c4 50
MazeTaka 0:c725bf9715c4 51 void readCANData()
MazeTaka 0:c725bf9715c4 52 {
MazeTaka 0:c725bf9715c4 53 CANMessage can_msg;
MazeTaka 0:c725bf9715c4 54 if(can.read(can_msg)) {
MazeTaka 0:c725bf9715c4 55 if(can_msg.id==CAN_ID) {
MazeTaka 0:c725bf9715c4 56 unpack_cmd(can_msg,&controller);
kageyuta 1:a636d553c70a 57 received = true;
MazeTaka 0:c725bf9715c4 58 }
MazeTaka 0:c725bf9715c4 59 }
kageyuta 1:a636d553c70a 60 }